diff --git a/.old/tracking/mpc_demo/cvxpy_mpc.py b/.old/tracking/mpc_demo/cvxpy_mpc.py new file mode 100755 index 0000000..98a5e83 --- /dev/null +++ b/.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 diff --git a/mpc_demo_v2/data/checker_blue.png b/.old/tracking/mpc_demo/data/checker_blue.png similarity index 100% rename from mpc_demo_v2/data/checker_blue.png rename to .old/tracking/mpc_demo/data/checker_blue.png diff --git a/mpc_demo_v2/data/plane.mtl b/.old/tracking/mpc_demo/data/plane.mtl similarity index 100% rename from mpc_demo_v2/data/plane.mtl rename to .old/tracking/mpc_demo/data/plane.mtl diff --git a/mpc_demo_v2/data/plane.obj b/.old/tracking/mpc_demo/data/plane.obj similarity index 100% rename from mpc_demo_v2/data/plane.obj rename to .old/tracking/mpc_demo/data/plane.obj diff --git a/mpc_demo_v2/data/plane.urdf b/.old/tracking/mpc_demo/data/plane.urdf similarity index 100% rename from mpc_demo_v2/data/plane.urdf rename to .old/tracking/mpc_demo/data/plane.urdf diff --git a/mpc_demo_v2/data/turtlebot.urdf b/.old/tracking/mpc_demo/data/turtlebot.urdf similarity index 100% rename from mpc_demo_v2/data/turtlebot.urdf rename to .old/tracking/mpc_demo/data/turtlebot.urdf diff --git a/mpc_demo_v2/data/turtlebot/kobuki_description/meshes/images/main_body.jpg b/.old/tracking/mpc_demo/data/turtlebot/kobuki_description/meshes/images/main_body.jpg similarity index 100% rename from mpc_demo_v2/data/turtlebot/kobuki_description/meshes/images/main_body.jpg rename to .old/tracking/mpc_demo/data/turtlebot/kobuki_description/meshes/images/main_body.jpg diff --git a/mpc_demo_v2/data/turtlebot/kobuki_description/meshes/images/wheel.jpg b/.old/tracking/mpc_demo/data/turtlebot/kobuki_description/meshes/images/wheel.jpg similarity index 100% rename from mpc_demo_v2/data/turtlebot/kobuki_description/meshes/images/wheel.jpg rename to .old/tracking/mpc_demo/data/turtlebot/kobuki_description/meshes/images/wheel.jpg diff --git a/mpc_demo_v2/data/turtlebot/kobuki_description/meshes/main_body.dae b/.old/tracking/mpc_demo/data/turtlebot/kobuki_description/meshes/main_body.dae similarity index 100% rename from mpc_demo_v2/data/turtlebot/kobuki_description/meshes/main_body.dae rename to .old/tracking/mpc_demo/data/turtlebot/kobuki_description/meshes/main_body.dae diff --git a/mpc_demo_v2/data/turtlebot/kobuki_description/meshes/wheel.dae b/.old/tracking/mpc_demo/data/turtlebot/kobuki_description/meshes/wheel.dae similarity index 100% rename from mpc_demo_v2/data/turtlebot/kobuki_description/meshes/wheel.dae rename to .old/tracking/mpc_demo/data/turtlebot/kobuki_description/meshes/wheel.dae diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/sensors/asus_xtion_pro_live.dae b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/sensors/asus_xtion_pro_live.dae similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/sensors/asus_xtion_pro_live.dae rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/sensors/asus_xtion_pro_live.dae diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/sensors/asus_xtion_pro_live.png b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/sensors/asus_xtion_pro_live.png similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/sensors/asus_xtion_pro_live.png rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/sensors/asus_xtion_pro_live.png diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/sensors/kinect.dae b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/sensors/kinect.dae similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/sensors/kinect.dae rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/sensors/kinect.dae diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/sensors/kinect.jpg b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/sensors/kinect.jpg similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/sensors/kinect.jpg rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/sensors/kinect.jpg diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/sensors/kinect.tga b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/sensors/kinect.tga similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/sensors/kinect.tga rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/sensors/kinect.tga diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/_DS_Store b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/_DS_Store similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/_DS_Store rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/_DS_Store diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/1f_pole.jpg b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/1f_pole.jpg similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/1f_pole.jpg rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/1f_pole.jpg diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/1f_stack.jpg b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/1f_stack.jpg similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/1f_stack.jpg rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/1f_stack.jpg diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/2f_pole.jpg b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/2f_pole.jpg similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/2f_pole.jpg rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/2f_pole.jpg diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/2f_stack.jpg b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/2f_stack.jpg similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/2f_stack.jpg rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/2f_stack.jpg diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_pole.jpg b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_pole.jpg similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_pole.jpg rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_pole.jpg diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_stack.jpg b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_stack.jpg similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_stack.jpg rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_stack.jpg diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_stack1.jpg b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_stack1.jpg similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_stack1.jpg rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_stack1.jpg diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/kinect_pole.jpg b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/kinect_pole.jpg similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/kinect_pole.jpg rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/kinect_pole.jpg diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/kinect_pole_old.jpg b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/kinect_pole_old.jpg similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/kinect_pole_old.jpg rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/kinect_pole_old.jpg diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_bottom.dae b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_bottom.dae similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_bottom.dae rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_bottom.dae diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_middle.dae b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_middle.dae similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_middle.dae rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_middle.dae diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_top.dae b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_top.dae similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_top.dae rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_top.dae diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_kinect.dae b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_kinect.dae similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_kinect.dae rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_kinect.dae diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_middle.dae b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_middle.dae similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_middle.dae rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_middle.dae diff --git a/mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_top.dae b/.old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_top.dae similarity index 100% rename from mpc_demo_v2/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_top.dae rename to .old/tracking/mpc_demo/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_top.dae diff --git a/mpc_demo_v2/mpc_demo_nosim.py b/.old/tracking/mpc_demo/mpc_demo_nosim.py similarity index 89% rename from mpc_demo_v2/mpc_demo_nosim.py rename to .old/tracking/mpc_demo/mpc_demo_nosim.py index a3704c9..a0b094b 100755 --- a/mpc_demo_v2/mpc_demo_nosim.py +++ b/.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() diff --git a/mpc_demo_v2/mpc_demo_pybullet.py b/.old/tracking/mpc_demo/mpc_demo_pybullet.py similarity index 100% rename from mpc_demo_v2/mpc_demo_pybullet.py rename to .old/tracking/mpc_demo/mpc_demo_pybullet.py diff --git a/.old/tracking/mpc_demo/utils.py b/.old/tracking/mpc_demo/utils.py new file mode 100755 index 0000000..5daa3dd --- /dev/null +++ b/.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)) diff --git a/notebooks/MPC_tracking_cvxpy.ipynb b/.old/tracking/notebooks/MPC_tracking_cvxpy.ipynb similarity index 100% rename from notebooks/MPC_tracking_cvxpy.ipynb rename to .old/tracking/notebooks/MPC_tracking_cvxpy.ipynb diff --git a/.old/tracking/notebooks/img/mpc_block_diagram.png b/.old/tracking/notebooks/img/mpc_block_diagram.png new file mode 100644 index 0000000..7f322d1 Binary files /dev/null and b/.old/tracking/notebooks/img/mpc_block_diagram.png differ diff --git a/.old/tracking/notebooks/img/mpc_t.png b/.old/tracking/notebooks/img/mpc_t.png new file mode 100644 index 0000000..8ed78b4 Binary files /dev/null and b/.old/tracking/notebooks/img/mpc_t.png differ diff --git a/mpc_demo/.ipynb_checkpoints/main-checkpoint.py b/mpc_demo/.ipynb_checkpoints/main-checkpoint.py deleted file mode 100755 index a05b363..0000000 --- a/mpc_demo/.ipynb_checkpoints/main-checkpoint.py +++ /dev/null @@ -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() diff --git a/mpc_demo/__pycache__/cvxpy_mpc.cpython-35.pyc b/mpc_demo/__pycache__/cvxpy_mpc.cpython-35.pyc deleted file mode 100644 index 968bdde..0000000 Binary files a/mpc_demo/__pycache__/cvxpy_mpc.cpython-35.pyc and /dev/null differ diff --git a/mpc_demo/__pycache__/cvxpy_mpc.cpython-36.pyc b/mpc_demo/__pycache__/cvxpy_mpc.cpython-36.pyc deleted file mode 100644 index f0abe3a..0000000 Binary files a/mpc_demo/__pycache__/cvxpy_mpc.cpython-36.pyc and /dev/null differ diff --git a/mpc_demo/__pycache__/utils.cpython-35.pyc b/mpc_demo/__pycache__/utils.cpython-35.pyc deleted file mode 100644 index 8a9d396..0000000 Binary files a/mpc_demo/__pycache__/utils.cpython-35.pyc and /dev/null differ diff --git a/mpc_demo/__pycache__/utils.cpython-36.pyc b/mpc_demo/__pycache__/utils.cpython-36.pyc deleted file mode 100644 index 55f287c..0000000 Binary files a/mpc_demo/__pycache__/utils.cpython-36.pyc and /dev/null differ diff --git a/mpc_demo/cvxpy_mpc.py b/mpc_demo/cvxpy_mpc.py index 98a5e83..0a3da8d 100755 --- a/mpc_demo/cvxpy_mpc.py +++ b/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(), diff --git a/mpc_demo_v2/mpc_config.py b/mpc_demo/mpc_config.py similarity index 100% rename from mpc_demo_v2/mpc_config.py rename to mpc_demo/mpc_config.py diff --git a/mpc_demo/mpc_demo_nosim.py b/mpc_demo/mpc_demo_nosim.py index a0b094b..a3704c9 100755 --- a/mpc_demo/mpc_demo_nosim.py +++ b/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() diff --git a/mpc_demo/mpc_demo_pybullet.py b/mpc_demo/mpc_demo_pybullet.py index 347207b..910bf88 100644 --- a/mpc_demo/mpc_demo_pybullet.py +++ b/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() diff --git a/mpc_demo/utils.py b/mpc_demo/utils.py index 5daa3dd..f952e0b 100755 --- a/mpc_demo/utils.py +++ b/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) diff --git a/mpc_demo_v2/cvxpy_mpc.py b/mpc_demo_v2/cvxpy_mpc.py deleted file mode 100755 index 0a3da8d..0000000 --- a/mpc_demo_v2/cvxpy_mpc.py +++ /dev/null @@ -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 diff --git a/mpc_demo_v2/utils.py b/mpc_demo_v2/utils.py deleted file mode 100755 index f952e0b..0000000 --- a/mpc_demo_v2/utils.py +++ /dev/null @@ -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) diff --git a/notebooks/MPC_cte_cvxpy.ipynb b/notebooks/MPC_cte_cvxpy.ipynb index ee3c452..16a75c6 100644 --- a/notebooks/MPC_cte_cvxpy.ipynb +++ b/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}) $" ] }, { diff --git a/notebooks/MPC_racecar.ipynb b/notebooks/MPC_racecar.ipynb new file mode 100644 index 0000000..49a484b --- /dev/null +++ b/notebooks/MPC_racecar.ipynb @@ -0,0 +1,1179 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "from scipy.integrate import odeint\n", + "from scipy.interpolate import interp1d\n", + "import cvxpy as cp\n", + "\n", + "import matplotlib.pyplot as plt\n", + "plt.style.use(\"ggplot\")\n", + "\n", + "import time" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### kinematics model equations\n", + "\n", + "The variables of the model are:\n", + "\n", + "* $x$ coordinate of the robot\n", + "* $y$ coordinate of the robot\n", + "* $v$ velocity of the robot\n", + "* $\\theta$ heading of the robot\n", + "\n", + "The inputs of the model are:\n", + "\n", + "* $v$ linear velocity of the robot\n", + "* $\\delta$ angular velocity of the robot\n", + "\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{v} = a$\n", + "* $\\dot{\\theta} = \\frac{v\\tan{\\delta}}{L}$\n", + "\n", + "Discretize with forward Euler Integration for time step dt:\n", + "\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", + "* ${v_{t+1}} = v_{t} + a_tdt$\n", + "* ${\\theta_{t+1}} = \\theta_{t} + \\frac{v\\tan{\\delta}}{L} dt$\n", + "\n", + "----------------------\n", + "\n", + "The Model is **non-linear** and **time variant**, but the Numerical Optimizer requires a Linear sets of equations. To approximate the equivalent **LTI** State space model, the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$ (at each time step):\n", + "\n", + "$ f(x,u) \\approx f(\\bar{x},\\bar{u}) + \\frac{\\partial f(x,u)}{\\partial x}|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + \\frac{\\partial f(x,u)}{\\partial u}|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n", + "\n", + "This can be rewritten usibg the State Space model form Ax+Bu :\n", + "\n", + "$ f(\\bar{x},\\bar{u}) + A|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + B|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n", + "\n", + "Where:\n", + "\n", + "$\n", + "A =\n", + "\\quad\n", + "\\begin{bmatrix}\n", + "\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial \\theta} \\\\\n", + "\\end{bmatrix}\n", + "\\quad\n", + "=\n", + "\\quad\n", + "\\begin{bmatrix}\n", + "0 & 0 & -v\\sin{\\theta} \\\\\n", + "0 & 0 & v\\cos{\\theta} \\\\\n", + "0 & 0 & 0\\\\\n", + "\\end{bmatrix}\n", + "\\quad\n", + "$\n", + "\n", + "and\n", + "\n", + "$\n", + "B = \n", + "\\quad\n", + "\\begin{bmatrix}\n", + "\\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial \\delta} \\\\\n", + "\\end{bmatrix}\n", + "\\quad\n", + "= \n", + "\\quad\n", + "\\begin{bmatrix}\n", + "\\cos{\\theta} & 0 \\\\\n", + "\\sin{\\theta} & 0 \\\\\n", + "\\frac{\\tan{\\delta}}{L} & \\frac{v}{L{\\cos{\\delta}}^2}\n", + "\\end{bmatrix}\n", + "\\quad\n", + "$\n", + "\n", + "are the *Jacobians*.\n", + "\n", + "\n", + "\n", + "So the discretized model is given by:\n", + "\n", + "$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A|_{x=\\bar{x}}(x_t-\\bar{x}) + B|_{u=\\bar{u}}(u_t-\\bar{u}) )dt $\n", + "\n", + "$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\n", + "\n", + "The LTI-equivalent kinematics model is:\n", + "\n", + "$ x_{t+1} = A'x_t + B' u_t + C' $\n", + "\n", + "with:\n", + "\n", + "$ A' = I+dtA|_{x=\\bar{x},u=\\bar{u}} $\n", + "\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}) $" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "-----------------\n", + "[About Taylor Series Expansion](https://courses.engr.illinois.edu/ece486/fa2017/documents/lecture_notes/state_space_p2.pdf):\n", + "\n", + "In order to linearize general nonlinear systems, we will use the Taylor Series expansion of functions.\n", + "\n", + "Typically it is possible to assume that the system is operating about some nominal\n", + "state solution $\\bar{x}$ (possibly requires a nominal input $\\bar{u}$) called **equilibrium point**.\n", + "\n", + "Recall that the Taylor Series expansion of f(x) around the\n", + "point $\\bar{x}$ is given by:\n", + "\n", + "$f(x)=f(\\bar{x}) + \\frac{df(x)}{dx}|_{x=\\bar{x}}(x-\\bar{x})$ + higher order terms...\n", + "\n", + "For x sufficiently close to $\\bar{x}$, these higher order terms will be very close to zero, and so we can drop them.\n", + "\n", + "The extension to functions of multiple states and inputs is very similar to the above procedure.Suppose the evolution of state x\n", + "is given by:\n", + "\n", + "$\\dot{x} = f(x1, x2, . . . , xn, u1, u2, . . . , um) = Ax+Bu$\n", + "\n", + "Where:\n", + "\n", + "$ A =\n", + "\\quad\n", + "\\begin{bmatrix}\n", + "\\frac{\\partial f(x,u)}{\\partial x1} & ... & \\frac{\\partial f(x,u)}{\\partial xn} \\\\\n", + "\\end{bmatrix}\n", + "\\quad\n", + "$ and $ B = \\quad\n", + "\\begin{bmatrix}\n", + "\\frac{\\partial f(x,u)}{\\partial u1} & ... & \\frac{\\partial f(x,u)}{\\partial um} \\\\\n", + "\\end{bmatrix}\n", + "\\quad $\n", + "\n", + "Then:\n", + "\n", + "$f(x,u)=f(\\bar{x},\\bar{u}) + \\frac{df(x,u)}{dx}|_{x=\\bar{x}}(x-\\bar{x}) + \\frac{df(x,u)}{du}|_{u=\\bar{u}}(u-\\bar{u}) = f(\\bar{x},\\bar{u}) + A_{x=\\bar{x}}(x-\\bar{x}) + B_{u=\\bar{u}}(u-\\bar{u})$\n", + "\n", + "-----------------" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Kinematics Model" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "# Control problem statement.\n", + "\n", + "N = 4 #number of state variables\n", + "M = 2 #number of control variables\n", + "T = 20 #Prediction Horizon\n", + "dt = 0.25 #discretization step" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "def get_linear_model(x_bar,u_bar):\n", + " \"\"\"\n", + " \"\"\"\n", + " L=0.3\n", + " \n", + " x = x_bar[0]\n", + " y = x_bar[1]\n", + " v = x_bar[2]\n", + " theta = x_bar[3]\n", + " \n", + " a = u_bar[0]\n", + " delta = u_bar[1]\n", + " \n", + " A = np.zeros((N,N))\n", + " A[0,2]=np.cos(theta)\n", + " A[1,2]=np.sin(theta)\n", + " A[0,3]=-v*np.sin(theta)\n", + " A[1,3]=v*np.cos(theta)\n", + " A[3,2]=v*np.tan(delta)/L\n", + " A_lin=np.eye(N)+dt*A\n", + " \n", + " B = np.zeros((N,M))\n", + " B[0,0]=np.cos(theta)\n", + " B[1,0]=np.sin(theta)\n", + " B[2,0]=1\n", + " B[3,1]=v/L*np.cos(delta)**2\n", + " B_lin=dt*B\n", + " \n", + " f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n", + " C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n", + " \n", + " return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Motion Prediction: using scipy intergration" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "# Define process model\n", + "# This uses the continuous model \n", + "def kinematics_model(x,t,u):\n", + " \"\"\"\n", + " \"\"\"\n", + " \n", + " #[x,y,v,th]\n", + " #[a,d]\n", + " \n", + " L=0.3\n", + " dxdt = x[2]*np.cos(x[3])\n", + " dydt = x[2]*np.sin(x[3])\n", + " dvdt = u[0]\n", + " dthetadt = x[2]*np.tan(u[1])/L\n", + "\n", + " dqdt = [dxdt,\n", + " dydt,\n", + " dvdt,\n", + " dthetadt]\n", + "\n", + " return dqdt\n", + "\n", + "def predict(x0,u):\n", + " \"\"\"\n", + " \"\"\"\n", + " \n", + " x_bar = np.zeros((N,T+1))\n", + " \n", + " x_bar[:,0] = x0\n", + " \n", + " # solve ODE\n", + " for t in range(1,T+1):\n", + "\n", + " tspan = [0,dt]\n", + " x_next = odeint(kinematics_model,\n", + " x0,\n", + " tspan,\n", + " args=(u[:,t-1],))\n", + "\n", + " x0 = x_next[1]\n", + " x_bar[:,t]=x_next[1]\n", + " \n", + " return x_bar" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Validate the model, here the status w.r.t a straight line with constant heading 0" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "CPU times: user 3.65 ms, sys: 0 ns, total: 3.65 ms\n", + "Wall time: 3.47 ms\n" + ] + } + ], + "source": [ + "%%time\n", + "\n", + "u_bar = np.zeros((M,T))\n", + "u_bar[0,:] = 0.2 #m/ss\n", + "u_bar[1,:] = np.radians(-np.pi/4) #rad\n", + "\n", + "x0 = np.zeros(N)\n", + "x0[0] = 0\n", + "x0[1] = 1\n", + "x0[2] = 0\n", + "x0[3] = np.radians(0)\n", + "\n", + "x_bar=predict(x0,u_bar)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Check the model prediction" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "#plot trajectory\n", + "plt.subplot(2, 2, 1)\n", + "plt.plot(x_bar[0,:],x_bar[1,:])\n", + "plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n", + "plt.axis('equal')\n", + "plt.ylabel('y')\n", + "plt.xlabel('x')\n", + "\n", + "plt.subplot(2, 2, 2)\n", + "plt.plot(np.degrees(x_bar[2,:]))\n", + "plt.ylabel('theta(t) [deg]')\n", + "#plt.xlabel('time')\n", + "\n", + "\n", + "plt.tight_layout()\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Motion Prediction: using the state space model" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "CPU times: user 2.48 ms, sys: 0 ns, total: 2.48 ms\n", + "Wall time: 1.85 ms\n" + ] + } + ], + "source": [ + "%%time\n", + "\n", + "u_bar = np.zeros((M,T))\n", + "u_bar[0,:] = 0.2 #m/s\n", + "u_bar[1,:] = np.radians(-np.pi/4) #rad\n", + "\n", + "x0 = np.zeros(N)\n", + "x0[0] = 0\n", + "x0[1] = 1\n", + "x0[2] = 0\n", + "x0[3] = np.radians(0)\n", + "\n", + "x_bar=np.zeros((N,T+1))\n", + "x_bar[:,0]=x0\n", + "\n", + "for t in range (1,T+1):\n", + " xt=x_bar[:,t-1].reshape(N,1)\n", + " ut=u_bar[:,t-1].reshape(M,1)\n", + " \n", + " A,B,C=get_linear_model(xt,ut)\n", + " \n", + " xt_plus_one = np.dot(A,xt)+np.dot(B,ut)+C\n", + " \n", + " x_bar[:,t]= np.squeeze(xt_plus_one)" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "#plot trajectory\n", + "plt.subplot(2, 2, 1)\n", + "plt.plot(x_bar[0,:],x_bar[1,:])\n", + "plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n", + "plt.axis('equal')\n", + "plt.ylabel('y')\n", + "plt.xlabel('x')\n", + "\n", + "plt.subplot(2, 2, 2)\n", + "plt.plot(np.degrees(x_bar[2,:]))\n", + "plt.ylabel('theta(t)')\n", + "#plt.xlabel('time')\n", + "\n", + "\n", + "plt.tight_layout()\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The results are the same as expected, so the linearized model is equivalent as expected." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## PRELIMINARIES" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [], + "source": [ + "def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n", + " final_xp=[]\n", + " final_yp=[]\n", + " delta = step #[m]\n", + "\n", + " for idx in range(len(start_xp)-1):\n", + " 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)))\n", + "\n", + " interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n", + " \n", + " fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n", + " fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n", + " \n", + " final_xp=np.append(final_xp,fx(interp_range))\n", + " final_yp=np.append(final_yp,fy(interp_range))\n", + "\n", + " return np.vstack((final_xp,final_yp))\n", + "\n", + "def get_nn_idx(state,path):\n", + "\n", + " dx = state[0]-path[0,:]\n", + " dy = state[1]-path[1,:]\n", + " dist = np.sqrt(dx**2 + dy**2)\n", + " nn_idx = np.argmin(dist)\n", + "\n", + " try:\n", + " v = [path[0,nn_idx+1] - path[0,nn_idx],\n", + " path[1,nn_idx+1] - path[1,nn_idx]] \n", + " v /= np.linalg.norm(v)\n", + "\n", + " d = [path[0,nn_idx] - state[0],\n", + " path[1,nn_idx] - state[1]]\n", + "\n", + " if np.dot(d,v) > 0:\n", + " target_idx = nn_idx\n", + " else:\n", + " target_idx = nn_idx+1\n", + "\n", + " except IndexError as e:\n", + " target_idx = nn_idx\n", + "\n", + " return target_idx\n", + "\n", + "def road_curve(state,track):\n", + " \n", + " #given vehicle pos find lookahead waypoints\n", + " nn_idx=get_nn_idx(state,track)-1\n", + " LOOKAHED=6\n", + " lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]\n", + "\n", + " #trasform lookahead waypoints to vehicle ref frame\n", + " dx = lk_wp[0,:] - state[0]\n", + " dy = lk_wp[1,:] - state[1]\n", + "\n", + " wp_vehicle_frame = np.vstack(( dx * np.cos(-state[3]) - dy * np.sin(-state[3]),\n", + " dy * np.cos(-state[3]) + dx * np.sin(-state[3]) ))\n", + "\n", + " #fit poly\n", + " return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 3, rcond=None, full=False, w=None, cov=False)\n", + "\n", + "def f(x,coeff):\n", + " return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)\n", + "\n", + "# def f(x,coeff):\n", + "# 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)\n", + "\n", + "def df(x,coeff):\n", + " return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n", + "# def df(x,coeff):\n", + "# 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)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### MPC Problem formulation\n", + "\n", + "**Model Predictive Control** refers to the control approach of **numerically** solving a optimization problem at each time step. \n", + "\n", + "The controller generates a control signal over a fixed lenght T (Horizon) at each time step." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "![mpc](img/mpc_block_diagram.png)\n", + "\n", + "![mpc](img/mpc_t.png)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "#### Linear MPC Formulation\n", + "\n", + "Linear MPC makes use of the **LTI** (Linear time invariant) discrete state space model, wich represents a motion model used for Prediction.\n", + "\n", + "$x_{t+1} = Ax_t + Bu_t$\n", + "\n", + "The LTI formulation means that **future states** are linearly related to the current state and actuator signal. Hence, the MPC seeks to find a **control policy** U over a finite lenght horizon.\n", + "\n", + "$U={u_{t|t}, u_{t+1|t}, ...,u_{t+T|t}}$\n", + "\n", + "The objective function used minimize (drive the state to 0) is:\n", + "\n", + "$\n", + "\\begin{equation}\n", + "\\begin{aligned}\n", + "\\min_{} \\quad & \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}\\\\\n", + "\\textrm{s.t.} \\quad & x(0) = x0\\\\\n", + " & x_{j+1|t} = Ax_{j|t}+Bu_{j|t}) \\quad \\textrm{for} \\quad t this is the value of the fitted polynomial\n", + "\n", + "* **heading error** epsi: desired heading - heading of vehicle -> is the inclination of tangent to the fitted polynomial\n", + "\n", + "Then using the fitted polynomial representation in vehicle frame the errors can be easily computed as:\n", + "\n", + "$\n", + "cte = f(px) \\\\\n", + "\\psi = -atan(f`(px)) \\\\\n", + "$" + ] + }, + { + "cell_type": "code", + "execution_count": 22, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "-----------------------------------------------------------------\n", + " OSQP v0.6.0 - Operator Splitting QP Solver\n", + " (c) Bartolomeo Stellato, Goran Banjac\n", + " University of Oxford - Stanford University 2019\n", + "-----------------------------------------------------------------\n", + "problem: variables n = 282, constraints m = 364\n", + " nnz(P) + nnz(A) = 975\n", + "settings: linear system solver = qdldl,\n", + " eps_abs = 1.0e-04, eps_rel = 1.0e-04,\n", + " eps_prim_inf = 1.0e-04, eps_dual_inf = 1.0e-04,\n", + " rho = 1.00e-01 (adaptive),\n", + " sigma = 1.00e-06, alpha = 1.60, max_iter = 10000\n", + " check_termination: on (interval 25),\n", + " scaling: on, scaled_termination: off\n", + " warm start: on, polish: on, time_limit: off\n", + "\n", + "iter objective pri res dua res rho time\n", + " 1 0.0000e+00 1.00e+00 1.00e+02 1.00e-01 3.06e-04s\n", + " 50 3.9564e-01 4.47e-10 6.69e-10 1.00e-01 6.77e-04s\n", + "\n", + "status: solved\n", + "solution polish: unsuccessful\n", + "number of iterations: 50\n", + "optimal objective: 0.3956\n", + "run time: 9.19e-04s\n", + "optimal rho estimate: 4.67e-02\n", + "\n", + "CPU times: user 284 ms, sys: 0 ns, total: 284 ms\n", + "Wall time: 281 ms\n" + ] + } + ], + "source": [ + "%%time\n", + "\n", + "MAX_SPEED = 1.25\n", + "MIN_SPEED = 0.75\n", + "MAX_STEER = 1.57\n", + "MAX_ACC = 1.0\n", + "\n", + "track = compute_path_from_wp([0,3],\n", + " [0,0],0.5)\n", + "\n", + "# Starting Condition\n", + "x0 = np.zeros(N)\n", + "x0[0] = 0\n", + "x0[1] = -0.25\n", + "x0[2] = 1\n", + "x0[3] = np.radians(-0)\n", + " \n", + "#starting guess\n", + "u_bar = np.zeros((M,T))\n", + "u_bar[0,:]=0.01\n", + "u_bar[1,:]=0.01\n", + "\n", + " \n", + "K=road_curve(x0,track)\n", + "\n", + "# dynamics starting state w.r.t vehicle frame\n", + "x_bar=np.zeros((N,T+1))\n", + "x_bar[2]=x0[2]\n", + "#prediction for linearization of costrains\n", + "for t in range (1,T+1):\n", + " xt=x_bar[:,t-1].reshape(N,1)\n", + " ut=u_bar[:,t-1].reshape(M,1)\n", + " A,B,C=get_linear_model(xt,ut)\n", + " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", + " x_bar[:,t]= xt_plus_one\n", + " \n", + "\n", + "x = cp.Variable((N, T+1))\n", + "u = cp.Variable((M, T))\n", + "\n", + "#CVXPY Linear MPC problem statement\n", + "cost = 0\n", + "constr = []\n", + "\n", + "for t in range(T):\n", + " \n", + " cost += 1*cp.sum_squares(x[3,t]-np.arctan(df(x_bar[0,t],K))) # psi\n", + " cost += 1*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n", + " # Actuation effort\n", + " cost += cp.quad_form( u[:, t],1*np.eye(M))\n", + " \n", + " # Actuation rate of change\n", + " if t < (T - 1):\n", + " cost += cp.quad_form(u[:, t + 1] - u[:, t], 1*np.eye(M))\n", + " \n", + " # KINEMATICS constrains\n", + " A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])\n", + " constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", + " \n", + "# sums problem objectives and concatenates constraints.\n", + "constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n", + "constr += [x[2, :] <= MAX_SPEED]\n", + "constr += [x[2, :] >= MIN_SPEED]\n", + "constr += [cp.abs(u[0, :]) <= MAX_ACC]\n", + "constr += [cp.abs(u[1, :]) <= MAX_STEER]\n", + "\n", + "prob = cp.Problem(cp.Minimize(cost), constr)\n", + "solution = prob.solve(solver=cp.OSQP, verbose=True)" + ] + }, + { + "cell_type": "code", + "execution_count": 24, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "x_mpc=np.array(x.value[0, :]).flatten()\n", + "y_mpc=np.array(x.value[1, :]).flatten()\n", + "theta_mpc=np.array(x.value[2, :]).flatten()\n", + "v_mpc=np.array(u.value[0, :]).flatten()\n", + "w_mpc=np.array(u.value[1, :]).flatten()\n", + "\n", + "#simulate robot state trajectory for optimized U\n", + "x_traj=predict(x0, np.vstack((v_mpc,w_mpc)))\n", + "\n", + "#plt.figure(figsize=(15,10))\n", + "#plot trajectory\n", + "plt.subplot(2, 2, 1)\n", + "plt.plot(track[0,:],track[1,:],\"b+\")\n", + "plt.plot(x_traj[0,:],x_traj[1,:])\n", + "plt.axis(\"equal\")\n", + "plt.ylabel('y')\n", + "plt.xlabel('x')\n", + "\n", + "#plot v(t)\n", + "plt.subplot(2, 2, 2)\n", + "plt.plot(v_mpc)\n", + "plt.ylabel('v(t)')\n", + "#plt.xlabel('time')\n", + "\n", + "#plot w(t)\n", + "# plt.subplot(2, 2, 3)\n", + "# plt.plot(w_mpc)\n", + "# plt.ylabel('w(t)')\n", + "#plt.xlabel('time')\n", + "\n", + "# plt.subplot(2, 2, 3)\n", + "# plt.plot(psi_mpc)\n", + "# plt.ylabel('psi(t)')\n", + "\n", + "# plt.subplot(2, 2, 4)\n", + "# plt.plot(cte_mpc)\n", + "# plt.ylabel('cte(t)')\n", + "\n", + "plt.tight_layout()\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "full track demo" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "track = compute_path_from_wp([0,3,4,6,10,13],\n", + " [0,0,2,4,3,3],0.25)\n", + "\n", + "# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n", + "# [0,0,2.5,2.5,0,0,5,10],0.5)\n", + "\n", + "sim_duration = 80 #time steps\n", + "opt_time=[]\n", + "\n", + "x_sim = np.zeros((N,sim_duration))\n", + "u_sim = np.zeros((M,sim_duration-1))\n", + "\n", + "MAX_SPEED = 1.25\n", + "MIN_SPEED = 0.75\n", + "MAX_STEER = 1.57\n", + "MAX_ACC = 1.0\n", + "\n", + "# Starting Condition\n", + "x0 = np.zeros(N)\n", + "x0[0] = 0\n", + "x0[1] = -0.25\n", + "x0[2] = np.radians(-0)\n", + "x_sim[:,0]=x0\n", + " \n", + "#starting guess\n", + "u_bar = np.zeros((M,T))\n", + "u_bar[0,:]=0.5*(MAX_SPEED+MIN_SPEED)\n", + "u_bar[1,:]=0.1\n", + "\n", + "for sim_time in range(sim_duration-1):\n", + " \n", + " iter_start=time.time()\n", + " \n", + " K=road_curve(x_sim[:,sim_time],track)\n", + " \n", + " # dynamics starting state w.r.t vehicle frame\n", + " x_bar=np.zeros((N,T+1))\n", + " \n", + " #prediction for linearization of costrains\n", + " for t in range (1,T+1):\n", + " xt=x_bar[:,t-1].reshape(N,1)\n", + " ut=u_bar[:,t-1].reshape(M,1)\n", + " A,B,C=get_linear_model(xt,ut)\n", + " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", + " x_bar[:,t]= xt_plus_one\n", + " \n", + " #CVXPY Linear MPC problem statement\n", + " cost = 0\n", + " constr = []\n", + " x = cp.Variable((N, T+1))\n", + " u = cp.Variable((M, T))\n", + " \n", + " #Prediction Horizon\n", + " for t in range(T):\n", + "\n", + " #cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n", + " cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi\n", + " cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n", + "\n", + " # Actuation rate of change\n", + " if t < (T - 1):\n", + " cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(M))\n", + " \n", + " # Actuation effort\n", + " cost += cp.quad_form( u[:, t],1*np.eye(M))\n", + " \n", + " # Kinrmatics Constrains (Linearized model)\n", + " A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])\n", + " constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", + "\n", + " # sums problem objectives and concatenates constraints.\n", + " constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n", + " constr += [u[0, :] <= MAX_SPEED]\n", + " constr += [u[0, :] >= MIN_SPEED]\n", + " constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n", + " \n", + " # Solve\n", + " prob = cp.Problem(cp.Minimize(cost), constr)\n", + " solution = prob.solve(solver=cp.OSQP, verbose=False)\n", + " \n", + " #retrieved optimized U and assign to u_bar to linearize in next step\n", + " u_bar=np.vstack((np.array(u.value[0, :]).flatten(),\n", + " (np.array(u.value[1, :]).flatten())))\n", + " \n", + " u_sim[:,sim_time] = u_bar[:,0]\n", + " \n", + " # Measure elpased time to get results from cvxpy\n", + " opt_time.append(time.time()-iter_start)\n", + " \n", + " # move simulation to t+1\n", + " tspan = [0,dt]\n", + " x_sim[:,sim_time+1] = odeint(kinematics_model,\n", + " x_sim[:,sim_time],\n", + " tspan,\n", + " args=(u_bar[:,0],))[1]\n", + " \n", + "print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", + " np.max(opt_time),\n", + " np.min(opt_time))) " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "#plot trajectory\n", + "grid = plt.GridSpec(2, 3)\n", + "\n", + "plt.figure(figsize=(15,10))\n", + "\n", + "plt.subplot(grid[0:2, 0:2])\n", + "plt.plot(track[0,:],track[1,:],\"b+\")\n", + "plt.plot(x_sim[0,:],x_sim[1,:])\n", + "plt.axis(\"equal\")\n", + "plt.ylabel('y')\n", + "plt.xlabel('x')\n", + "\n", + "plt.subplot(grid[0, 2])\n", + "plt.plot(u_sim[0,:])\n", + "plt.ylabel('v(t) [m/s]')\n", + "\n", + "plt.subplot(grid[1, 2])\n", + "plt.plot(np.degrees(u_sim[1,:]))\n", + "plt.ylabel('w(t) [deg/s]')\n", + "\n", + "plt.tight_layout()\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## OBSTACLE AVOIDANCE\n", + "see dccp paper for reference" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import dccp\n", + "track = compute_path_from_wp([0,3,4,6,10,13],\n", + " [0,0,2,4,3,3],0.25)\n", + "\n", + "obstacles=np.array([[4,6],[2,4]])\n", + "obstacle_radius=0.5\n", + "\n", + "def to_vehic_frame(pt,pos_x,pos_y,theta):\n", + " dx = pt[0] - pos_x\n", + " dy = pt[1] - pos_y\n", + "\n", + " return [dx * np.cos(-theta) - dy * np.sin(-theta),\n", + " dy * np.cos(-theta) + dx * np.sin(-theta)]\n", + " \n", + "# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n", + "# [0,0,2.5,2.5,0,0,5,10],0.5)\n", + "\n", + "sim_duration = 80 #time steps\n", + "opt_time=[]\n", + "\n", + "x_sim = np.zeros((N,sim_duration))\n", + "u_sim = np.zeros((M,sim_duration-1))\n", + "\n", + "MAX_SPEED = 1.25\n", + "MIN_SPEED = 0.75\n", + "MAX_STEER_SPEED = 1.57\n", + "\n", + "# Starting Condition\n", + "x0 = np.zeros(N)\n", + "x0[0] = 0\n", + "x0[1] = -0.25\n", + "x0[2] = np.radians(-0)\n", + "x_sim[:,0]=x0\n", + " \n", + "#starting guess\n", + "u_bar = np.zeros((M,T))\n", + "u_bar[0,:]=0.5*(MAX_SPEED+MIN_SPEED)\n", + "u_bar[1,:]=0.00\n", + "\n", + "for sim_time in range(sim_duration-1):\n", + " \n", + " iter_start=time.time()\n", + " \n", + " #compute coefficients\n", + " K=road_curve(x_sim[:,sim_time],track)\n", + " \n", + " #compute opstacles in ref frame\n", + " o_=[]\n", + " for j in range(2):\n", + " o_.append(to_vehic_frame(obstacles[:,j],x_sim[0,sim_time],x_sim[1,sim_time],x_sim[2,sim_time]) )\n", + " \n", + " # dynamics starting state w.r.t vehicle frame\n", + " x_bar=np.zeros((N,T+1))\n", + " \n", + " #prediction for linearization of costrains\n", + " for t in range (1,T+1):\n", + " xt=x_bar[:,t-1].reshape(N,1)\n", + " ut=u_bar[:,t-1].reshape(M,1)\n", + " A,B,C=get_linear_model(xt,ut)\n", + " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", + " x_bar[:,t]= xt_plus_one\n", + " \n", + " #CVXPY Linear MPC problem statement\n", + " cost = 0\n", + " constr = []\n", + " x = cp.Variable((N, T+1))\n", + " u = cp.Variable((M, T))\n", + " \n", + " #Prediction Horizon\n", + " for t in range(T):\n", + "\n", + " #cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n", + " cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi\n", + " cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n", + "\n", + " # Actuation rate of change\n", + " if t < (T - 1):\n", + " cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(M))\n", + " \n", + " # Actuation effort\n", + " cost += cp.quad_form( u[:, t],1*np.eye(M))\n", + " \n", + " # Kinrmatics Constrains (Linearized model)\n", + " A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])\n", + " constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", + " \n", + " # Obstacle Avoidance Contrains\n", + " for j in range(2):\n", + " constr += [ cp.norm(x[0:2,t]-o_[j],2) >= obstacle_radius ]\n", + "\n", + " # sums problem objectives and concatenates constraints.\n", + " constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n", + " constr += [u[0, :] <= MAX_SPEED]\n", + " constr += [u[0, :] >= MIN_SPEED]\n", + " constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n", + " \n", + " # Solve\n", + " prob = cp.Problem(cp.Minimize(cost), constr)\n", + " solution = prob.solve(method=\"dccp\", verbose=False)\n", + " \n", + " #retrieved optimized U and assign to u_bar to linearize in next step\n", + " u_bar=np.vstack((np.array(u.value[0, :]).flatten(),\n", + " (np.array(u.value[1, :]).flatten())))\n", + " \n", + " u_sim[:,sim_time] = u_bar[:,0]\n", + " \n", + " # Measure elpased time to get results from cvxpy\n", + " opt_time.append(time.time()-iter_start)\n", + " \n", + " # move simulation to t+1\n", + " tspan = [0,dt]\n", + " x_sim[:,sim_time+1] = odeint(kinematics_model,\n", + " x_sim[:,sim_time],\n", + " tspan,\n", + " args=(u_bar[:,0],))[1]\n", + " \n", + "print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", + " np.max(opt_time),\n", + " np.min(opt_time))) " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "#plot trajectory\n", + "grid = plt.GridSpec(2, 3)\n", + "\n", + "plt.figure(figsize=(15,10))\n", + "\n", + "ax=plt.subplot(grid[0:2, 0:2])\n", + "plt.plot(track[0,:],track[1,:],\"b+\")\n", + "plt.plot(x_sim[0,:],x_sim[1,:])\n", + "#obstacles\n", + "circle1=plt.Circle((obstacles[0,0], obstacles[1,0]), obstacle_radius, color='r')\n", + "circle2=plt.Circle((obstacles[0,1], obstacles[1,1]), obstacle_radius, color='r')\n", + "plt.axis(\"equal\")\n", + "plt.ylabel('y')\n", + "plt.xlabel('x')\n", + "\n", + "ax.add_artist(circle1)\n", + "ax.add_artist(circle2)\n", + "\n", + "plt.subplot(grid[0, 2])\n", + "plt.plot(u_sim[0,:])\n", + "plt.ylabel('v(t) [m/s]')\n", + "\n", + "plt.subplot(grid[1, 2])\n", + "plt.plot(np.degrees(u_sim[1,:]))\n", + "plt.ylabel('w(t) [deg/s]')\n", + "\n", + "\n", + "plt.tight_layout()\n", + "plt.show()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python [conda env:jupyter] *", + "language": "python", + "name": "conda-env-jupyter-py" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.3" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/notebooks/equations.ipynb b/notebooks/equations.ipynb index 4006b42..b210987 100644 --- a/notebooks/equations.ipynb +++ b/notebooks/equations.ipynb @@ -4,7 +4,9 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# STATE SPACE MODEL MATRICES" + "# STATE SPACE MODEL MATRICES\n", + "\n", + "### Diff drive" ] }, { @@ -13,15 +15,22 @@ "metadata": {}, "outputs": [ { - "ename": "ModuleNotFoundError", - "evalue": "No module named 'sympy'", - "output_type": "error", - "traceback": [ - "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mModuleNotFoundError\u001b[0m Traceback (most recent call last)", - "\u001b[0;32m\u001b[0m in \u001b[0;36m\u001b[0;34m\u001b[0m\n\u001b[0;32m----> 1\u001b[0;31m \u001b[0;32mimport\u001b[0m \u001b[0msympy\u001b[0m \u001b[0;32mas\u001b[0m \u001b[0msp\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 2\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 3\u001b[0m \u001b[0mx\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0my\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mtheta\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mpsi\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mcte\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mv\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mw\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0msp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msymbols\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m\"x y theta psi cte v w\"\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 4\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 5\u001b[0m gs = sp.Matrix([[ sp.cos(theta)*v],\n", - "\u001b[0;31mModuleNotFoundError\u001b[0m: No module named 'sympy'" - ] + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{matrix}0 & 0 & - v \\sin{\\left(\\theta \\right)} & 0 & 0\\\\0 & 0 & v \\cos{\\left(\\theta \\right)} & 0 & 0\\\\0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & v \\cos{\\left(\\psi \\right)} & 0\\end{matrix}\\right]$" + ], + "text/plain": [ + "Matrix([\n", + "[0, 0, -v*sin(theta), 0, 0],\n", + "[0, 0, v*cos(theta), 0, 0],\n", + "[0, 0, 0, 0, 0],\n", + "[0, 0, 0, 0, 0],\n", + "[0, 0, 0, v*cos(psi), 0]])" + ] + }, + "execution_count": 1, + "metadata": {}, + "output_type": "execute_result" } ], "source": [ @@ -43,9 +52,28 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 2, "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & 0\\\\\\sin{\\left(\\theta \\right)} & 0\\\\0 & 1\\\\0 & -1\\\\\\sin{\\left(\\psi \\right)} & 0\\end{matrix}\\right]$" + ], + "text/plain": [ + "Matrix([\n", + "[cos(theta), 0],\n", + "[sin(theta), 0],\n", + "[ 0, 1],\n", + "[ 0, -1],\n", + "[ sin(psi), 0]])" + ] + }, + "execution_count": 2, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "state = sp.Matrix([v,w])\n", "\n", @@ -55,9 +83,26 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 3, "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{matrix}1 & 0 & - dt v \\sin{\\left(\\theta \\right)}\\\\0 & 1 & dt v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 1\\end{matrix}\\right]$" + ], + "text/plain": [ + "Matrix([\n", + "[1, 0, -dt*v*sin(theta)],\n", + "[0, 1, dt*v*cos(theta)],\n", + "[0, 0, 1]])" + ] + }, + "execution_count": 3, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "import sympy as sp\n", "\n", @@ -75,9 +120,26 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 4, "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{matrix}dt \\cos{\\left(\\theta \\right)} & 0\\\\dt \\sin{\\left(\\theta \\right)} & 0\\\\0 & dt\\end{matrix}\\right]$" + ], + "text/plain": [ + "Matrix([\n", + "[dt*cos(theta), 0],\n", + "[dt*sin(theta), 0],\n", + "[ 0, dt]])" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "state = sp.Matrix([v,w])\n", "\n", @@ -85,6 +147,80 @@ "gs.jacobian(state)#.subs({x:0,y:0,theta:0})" ] }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Ackermann" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{matrix}0 & 0 & \\cos{\\left(\\theta \\right)} & - v \\sin{\\left(\\theta \\right)}\\\\0 & 0 & \\sin{\\left(\\theta \\right)} & v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 0 & 0\\\\0 & 0 & \\frac{\\tan{\\left(\\delta \\right)}}{L} & 0\\end{matrix}\\right]$" + ], + "text/plain": [ + "Matrix([\n", + "[0, 0, cos(theta), -v*sin(theta)],\n", + "[0, 0, sin(theta), v*cos(theta)],\n", + "[0, 0, 0, 0],\n", + "[0, 0, tan(delta)/L, 0]])" + ] + }, + "execution_count": 8, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "x,y,theta,v,delta,L,a = sp.symbols(\"x y theta v delta L a\")\n", + "\n", + "gs = sp.Matrix([[ sp.cos(theta)*v],\n", + " [ sp.sin(theta)*v],\n", + " [a],\n", + " [ v*sp.tan(delta)/L]])\n", + "\n", + "state = sp.Matrix([x,y,v,theta])\n", + "\n", + "#A\n", + "gs.jacobian(state)" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\left[\\begin{matrix}0 & 0\\\\0 & 0\\\\1 & 0\\\\0 & \\frac{v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]$" + ], + "text/plain": [ + "Matrix([\n", + "[0, 0],\n", + "[0, 0],\n", + "[1, 0],\n", + "[0, v*(tan(delta)**2 + 1)/L]])" + ] + }, + "execution_count": 7, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "state = sp.Matrix([a,delta])\n", + "\n", + "#B\n", + "gs.jacobian(state)#.subs({x:0,y:0,theta:0})" + ] + }, { "cell_type": "markdown", "metadata": {}, @@ -94,7 +230,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -147,18 +283,9 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "/home/marcello/miniconda3/envs/jupyter/lib/python3.8/site-packages/IPython/core/interactiveshell.py:3331: RankWarning: Polyfit may be poorly conditioned\n", - " exec(code_obj, self.user_global_ns, self.user_ns)\n" - ] - } - ], + "outputs": [], "source": [ "#define track\n", "wp=np.array([0,5,6,10,11,15, 0,0,2,2,0,4]).reshape(2,-1)\n", @@ -190,41 +317,18 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "array([ 0.10275887, 0.03660033, -0.21750601, 0.03551043, -0.53861442,\n", - " -0.58083993])" - ] - }, - "execution_count": 4, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ "coeff" ] }, { "cell_type": "code", - "execution_count": 13, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "data": { - "image/png": "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\n", - "text/plain": [ - "
" - ] - }, - "metadata": {}, - "output_type": "display_data" - } - ], + "outputs": [], "source": [ "import matplotlib.pyplot as plt\n", "plt.style.use(\"ggplot\")\n", diff --git a/racecar.py b/racecar.py new file mode 100644 index 0000000..be71a2f --- /dev/null +++ b/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)