import numpy as np import matplotlib.pyplot as plt N = 1000 # number of timesteps T = 50 # time will vary from 0 to T with step delt ts = np.linspace(0,T,N+1) delt = T/N gamma = .05 # damping, 0 is no damping A = np.zeros((4,4)) B = np.zeros((4,2)) C = np.zeros((2,4)) A[0,0] = 1 A[1,1] = 1 A[0,2] = (1-gamma*delt/2)*delt A[1,3] = (1-gamma*delt/2)*delt A[2,2] = 1 - gamma*delt A[3,3] = 1 - gamma*delt B[0,0] = delt**2/2 B[1,1] = delt**2/2 B[2,0] = delt B[3,1] = delt import scipy.sparse as ssp import scipy.sparse.linalg as sla x_0 = np.array([10, -20, 15, -5]) x_des = np.array([100, 50, 0, 0]) G = np.zeros((4,2*N)) for i in range(N): G[:, 2*i:2*(i+1)] = np.linalg.matrix_power(A,max(0,N-i-1))@B u_hat = sla.lsqr(G,x_des - np.linalg.matrix_power(A,N)@x_0)[0] u_vec = u_hat u_opt = u_vec.reshape(1000,2).T x_opt = np.zeros((4,N+1)) x_opt[:,0] = x_0 for t in range(N): x_opt[:,t+1] = A.dot(x_opt[:,t]) + B.dot(u_opt[:,t]) plt.figure(figsize=(14,9), dpi=100) plt.subplot(2,2,1) plt.plot(ts,x_opt[0,:]) plt.xlabel('time') plt.ylabel('x position') plt.grid() plt.subplot(2,2,2) plt.plot(ts,x_opt[1,:]) plt.xlabel('time') plt.ylabel('y position') plt.grid() plt.subplot(2,2,3) plt.plot(ts,x_opt[2,:]) plt.xlabel('time') plt.ylabel('x velocity') plt.grid() plt.subplot(2,2,4) plt.plot(ts,x_opt[3,:]) plt.xlabel('time') plt.ylabel('y velocity') plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.subplot(2,2,1) plt.plot(ts[:-1],u_opt[0,:]) plt.xlabel('time') plt.ylabel(r'$u_1$') plt.grid() plt.subplot(2,2,2) plt.plot(ts[:-1],u_opt[1,:]) plt.xlabel('time') plt.ylabel(r'$u_2$') plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.plot(x_opt[0,:],x_opt[1,:], label='Optimal trajectory') plt.plot(x_0[0], x_0[1], 'o', markersize=7, label='Initial position') plt.plot(x_des[0], x_des[1], '*', markersize=10, label='Target position') plt.title('Trajectory') plt.legend() for i in range(0,N-1,10): plt.arrow(x_opt[0,i], x_opt[1,i], 10*u_opt[0,i], 10*u_opt[1,i], head_width=0.2, width=0.2, fc='tab:red', ec='none') plt.axis('equal') plt.xlabel(r'$x$ position') plt.ylabel(r'$y$ position') plt.grid() plt.show() import cvxpy as cp x = cp.Variable((4,N+1)) # x_{0},...,x_{N} u = cp.Variable((2,N)) # u_{0},...,u_{N-1} obj = cp.Minimize(cp.sum_squares(u)) constr = [ x[:,-1] == x_des, x[:,0] == x_0 ] for t in range(N): constr += [ x[:,t+1] == A@x[:,t] + B@u[:,t] ] cp.Problem(obj, constr).solve(verbose=True) x_cp = np.array(x.value) u_cp = np.array(u.value) plt.figure(figsize=(14,9), dpi=100) plt.subplot(2,2,1) plt.plot(ts,x_opt[0,:]) plt.plot(ts,x_cp[0,:], '--') plt.xlabel('time') plt.ylabel('x position') plt.grid() plt.subplot(2,2,2) plt.plot(ts,x_opt[1,:]) plt.plot(ts,x_cp[1,:], '--') plt.xlabel('time') plt.ylabel('y position') plt.grid() plt.subplot(2,2,3) plt.plot(ts,x_opt[2,:]) plt.plot(ts,x_cp[2,:], '--') plt.xlabel('time') plt.ylabel('x velocity') plt.grid() plt.subplot(2,2,4) plt.plot(ts,x_opt[3,:]) plt.plot(ts,x_cp[3,:], '--') plt.xlabel('time') plt.ylabel('y velocity') plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.subplot(2,2,1) plt.plot(ts[:-1],u_opt[0,:]) plt.plot(ts[:-1],u_cp[0,:], '--') plt.xlabel('time') plt.ylabel(r'$u_1$') plt.grid() plt.subplot(2,2,2) plt.plot(ts[:-1],u_opt[1,:]) plt.plot(ts[:-1],u_cp[1,:], '--') plt.xlabel('time') plt.ylabel(r'$u_2$') plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.plot(x_opt[0,:],x_opt[1,:], label='Optimal trajectory (explicit)') plt.plot(x_cp[0,:],x_cp[1,:], '--', label='Optimal trajectory (cvxpy)') plt.plot(x_0[0], x_0[1], 'o', markersize=7, label='Initial position') plt.plot(x_des[0], x_des[1], '*', markersize=10, label='Target position') plt.title('Trajectory') plt.legend() for i in range(0,N-1,10): plt.arrow(x_opt[0,i], x_opt[1,i], 10*u_opt[0,i], 10*u_opt[1,i], head_width=0.2, width=0.2, fc='tab:red', ec='none') plt.arrow(x_cp[0,i], x_cp[1,i], 10*u_cp[0,i], 10*u_cp[1,i], head_width=0.2, width=0.2, fc='tab:red', ec='none') plt.axis('equal') plt.xlabel(r'$x$ position') plt.ylabel(r'$y$ position') plt.grid() plt.show() x_diff = x_cp - x_opt u_diff = u_cp - u_opt print (np.linalg.norm(x_diff)) print (np.linalg.norm(u_diff)) import cvxpy as cp ########################################## p_lb = np.array([ 0, -35]) p_ub = np.array([115, 70]) C = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) ########################################## x = cp.Variable((4,N+1)) # x_{0},...,x_{N} u = cp.Variable((2,N)) # u_{0},...,u_{N-1} obj = cp.Minimize(cp.sum_squares(u)) constr = [ x[:,-1] == x_des, x[:,0] == x_0 ] for t in range(N): constr += [ x[:,t+1] == A@x[:,t] + B@u[:,t] ] #################################################### constr += [ p_lb <= C@x[:,t+1], C@x[:,t+1] <= p_ub ] #################################################### cp.Problem(obj, constr).solve(verbose=True) x_cp = np.array(x.value) u_cp = np.array(u.value) plt.figure(figsize=(14,9), dpi=100) plt.subplot(2,2,1) plt.plot(ts,x_opt[0,:]) plt.plot(ts,x_cp[0,:], '--') plt.xlabel('time') plt.ylabel('x position') plt.grid() plt.subplot(2,2,2) plt.plot(ts,x_opt[1,:]) plt.plot(ts,x_cp[1,:], '--') plt.xlabel('time') plt.ylabel('y position') plt.grid() plt.subplot(2,2,3) plt.plot(ts,x_opt[2,:]) plt.plot(ts,x_cp[2,:], '--') plt.xlabel('time') plt.ylabel('x velocity') plt.grid() plt.subplot(2,2,4) plt.plot(ts,x_opt[3,:]) plt.plot(ts,x_cp[3,:], '--') plt.xlabel('time') plt.ylabel('y velocity') plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.subplot(2,2,1) plt.plot(ts[:-1],u_opt[0,:]) plt.plot(ts[:-1],u_cp[0,:], '--') plt.xlabel('time') plt.ylabel(r'$u_1$') plt.grid() plt.subplot(2,2,2) plt.plot(ts[:-1],u_opt[1,:]) plt.plot(ts[:-1],u_cp[1,:], '--') plt.xlabel('time') plt.ylabel(r'$u_2$') plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.plot(x_opt[0,:],x_opt[1,:], label='Optimal trajectory (unconstrained)', alpha=0.5) plt.plot(x_cp[0,:],x_cp[1,:], '--', label='Optimal trajectory (constrained)') plt.plot(x_0[0], x_0[1], 'o', markersize=7, label='Initial position') plt.plot(x_des[0], x_des[1], '*', markersize=10, label='Target position') plt.broken_barh([(p_lb[0], p_ub[0])], (p_lb[1], p_ub[1]-p_lb[1]), \ alpha = 0.1, label='Feasible region') plt.title('Trajectory') plt.legend() for i in range(0,N-1,10): plt.arrow(x_opt[0,i], x_opt[1,i], 10*u_opt[0,i], 10*u_opt[1,i], head_width=0.2, width=0.2, fc='tab:red', ec='none', alpha=0.5) plt.arrow(x_cp[0,i], x_cp[1,i], 10*u_cp[0,i], 10*u_cp[1,i], head_width=0.2, width=0.2, fc='tab:red', ec='none') plt.axis('equal') plt.xlabel(r'$x$ position') plt.ylabel(r'$y$ position') plt.grid() plt.show() import cvxpy as cp ############################# u_lb = np.array([-1, -1])*0.7 u_ub = np.array([ 1, 1])*0.7 ############################# p_lb = np.array([ 0, -35]) p_ub = np.array([115, 70]) C = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) x = cp.Variable((4,N+1)) # x_{0},...,x_{N} u = cp.Variable((2,N)) # u_{0},...,u_{N-1} obj = cp.Minimize(cp.sum_squares(u)) constr = [ x[:,-1] == x_des, x[:,0] == x_0 ] for t in range(N): constr += [ x[:,t+1] == A@x[:,t] + B@u[:,t] ] constr += [ p_lb <= C@x[:,t+1], C@x[:,t+1] <= p_ub ] ############################################ constr += [ u_lb <= u[:,t], u[:,t] <= u_ub ] ############################################ cp.Problem(obj, constr).solve(verbose=True) x_cp = np.array(x.value) u_cp = np.array(u.value) x_l2, u_l2 = x_cp, u_cp plt.figure(figsize=(14,9), dpi=100) plt.subplot(2,2,1) plt.plot(ts,x_opt[0,:]) plt.plot(ts,x_cp[0,:], '--') plt.xlabel('time') plt.ylabel('x position') plt.grid() plt.subplot(2,2,2) plt.plot(ts,x_opt[1,:]) plt.plot(ts,x_cp[1,:], '--') plt.xlabel('time') plt.ylabel('y position') plt.grid() plt.subplot(2,2,3) plt.plot(ts,x_opt[2,:]) plt.plot(ts,x_cp[2,:], '--') plt.xlabel('time') plt.ylabel('x velocity') plt.grid() plt.subplot(2,2,4) plt.plot(ts,x_opt[3,:]) plt.plot(ts,x_cp[3,:], '--') plt.xlabel('time') plt.ylabel('y velocity') plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.subplot(2,2,1) plt.plot(ts[:-1],u_opt[0,:]) plt.plot(ts[:-1],u_cp[0,:], '--') plt.xlabel('time') plt.ylabel(r'$u_1$') plt.grid() plt.subplot(2,2,2) plt.plot(ts[:-1],u_opt[1,:]) plt.plot(ts[:-1],u_cp[1,:], '--') plt.xlabel('time') plt.ylabel(r'$u_2$') plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.plot(x_opt[0,:],x_opt[1,:], label='Optimal trajectory (unconstrained)', alpha=0.5) plt.plot(x_cp[0,:],x_cp[1,:], '--', label='Optimal trajectory (constrained)') plt.plot(x_0[0], x_0[1], 'o', markersize=7, label='Initial position') plt.plot(x_des[0], x_des[1], '*', markersize=10, label='Target position') plt.broken_barh([(p_lb[0], p_ub[0])], (p_lb[1], p_ub[1]-p_lb[1]), \ alpha = 0.1, label='Feasible region') plt.title('Trajectory') plt.legend() for i in range(0,N-1,10): plt.arrow(x_opt[0,i], x_opt[1,i], 10*u_opt[0,i], 10*u_opt[1,i], head_width=0.2, width=0.2, fc='tab:red', ec='none', alpha=0.5) plt.arrow(x_cp[0,i], x_cp[1,i], 10*u_cp[0,i], 10*u_cp[1,i], head_width=0.2, width=0.2, fc='tab:red', ec='none') plt.axis('equal') plt.xlabel(r'$x$ position') plt.ylabel(r'$y$ position') plt.grid() plt.show() import cvxpy as cp u_lb = np.array([-1, -1])*0.7 u_ub = np.array([ 1, 1])*0.7 p_lb = np.array([ 0, -35]) p_ub = np.array([115, 70]) C = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) x = cp.Variable((4,N+1)) # x_{0},...,x_{N} u = cp.Variable((2,N)) # u_{0},...,u_{N-1} ############################# obj = cp.norm(cp.norm(u,2,axis=0),1) ############################# obj = cp.Minimize(obj) constr = [ x[:,-1] == x_des, x[:,0] == x_0 ] for t in range(N): constr += [ x[:,t+1] == A@x[:,t] + B@u[:,t] ] constr += [ p_lb <= C@x[:,t+1], C@x[:,t+1] <= p_ub ] ############################################ constr += [ u_lb <= u[:,t], u[:,t] <= u_ub ] ############################################ cp.Problem(obj, constr).solve(verbose=True) x_cp = np.array(x.value) u_cp = np.array(u.value) x_l1, u_l1 = x_cp, u_cp plt.figure(figsize=(14,9), dpi=100) plt.subplot(2,2,1) plt.plot(ts,x_l2[0,:]) plt.plot(ts,x_l1[0,:], '--') plt.xlabel('time') plt.ylabel('x position') plt.grid() plt.subplot(2,2,2) plt.plot(ts,x_l2[1,:]) plt.plot(ts,x_l1[1,:], '--') plt.xlabel('time') plt.ylabel('y position') plt.grid() plt.subplot(2,2,3) plt.plot(ts,x_l2[2,:]) plt.plot(ts,x_l1[2,:], '--') plt.xlabel('time') plt.ylabel('x velocity') plt.grid() plt.subplot(2,2,4) plt.plot(ts,x_l2[3,:]) plt.plot(ts,x_l1[3,:], '--') plt.xlabel('time') plt.ylabel('y velocity') plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.subplot(2,2,1) plt.plot(ts[:-1],u_l2[0,:]) plt.plot(ts[:-1],u_l1[0,:], '--') plt.xlabel('time') plt.ylabel(r'$u_1$') plt.grid() plt.subplot(2,2,2) plt.plot(ts[:-1],u_l2[1,:]) plt.plot(ts[:-1],u_l1[1,:], '--') plt.xlabel('time') plt.ylabel(r'$u_2$') plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.plot(x_l2[0,:],x_l2[1,:], label=r'Optimal trajectory ($\ell_2$ optimal)', alpha=0.5) plt.plot(x_l1[0,:],x_l1[1,:], '--', label=r'Optimal trajectory ($\ell_1$ optimal)') plt.plot(x_0[0], x_0[1], 'o', markersize=7, label='Initial position') plt.plot(x_des[0], x_des[1], '*', markersize=10, label='Target position') plt.broken_barh([(p_lb[0], p_ub[0])], (p_lb[1], p_ub[1]-p_lb[1]), \ alpha = 0.1, label='Feasible region') plt.title('Trajectory') plt.legend() for i in range(0,N-1,10): plt.arrow(x_l2[0,i], x_l2[1,i], 10*u_l2[0,i], 10*u_l2[1,i], head_width=0.2, width=0.2, fc='tab:red', ec='none', alpha=0.5) plt.arrow(x_l1[0,i], x_l1[1,i], 10*u_l1[0,i], 10*u_l1[1,i], head_width=0.2, width=0.2, fc='tab:red', ec='none') plt.axis('equal') plt.xlabel(r'$x$ position') plt.ylabel(r'$y$ position') plt.grid() plt.show() import cvxpy as cp u_lb = np.array([-1, -1])*0.7 u_ub = np.array([ 1, 1])*0.7 p_lb = np.array([ 0, -35]) p_ub = np.array([115, 70]) C = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) x = cp.Variable((4,N+1)) # x_{0},...,x_{N} u = cp.Variable((2,N)) # u_{0},...,u_{N-1} ################################################ obj = cp.max(cp.norm(u, 2, axis=0)) obj += 1e-4*cp.norm(cp.norm(u, 2, axis=0),1) obj = cp.Minimize(obj) ################################################ constr = [ x[:,-1] == x_des, x[:,0] == x_0 ] for t in range(N): constr += [ x[:,t+1] == A@x[:,t] + B@u[:,t] ] constr += [ p_lb <= C@x[:,t+1], C@x[:,t+1] <= p_ub ] constr += [ u_lb <= u[:,t], u[:,t] <= u_ub ] cp.Problem(obj, constr).solve(verbose=True) x_cp = np.array(x.value) u_cp = np.array(u.value) x_li, u_li = x_cp, u_cp plt.figure(figsize=(14,9), dpi=100) plt.subplot(2,2,1) plt.plot(ts,x_l2[0,:]) plt.plot(ts,x_li[0,:], '--') plt.xlabel('time') plt.ylabel('x position') plt.grid() plt.subplot(2,2,2) plt.plot(ts,x_l2[1,:]) plt.plot(ts,x_li[1,:], '--') plt.xlabel('time') plt.ylabel('y position') plt.grid() plt.subplot(2,2,3) plt.plot(ts,x_l2[2,:]) plt.plot(ts,x_li[2,:], '--') plt.xlabel('time') plt.ylabel('x velocity') plt.grid() plt.subplot(2,2,4) plt.plot(ts,x_l2[3,:]) plt.plot(ts,x_li[3,:], '--') plt.xlabel('time') plt.ylabel('y velocity') plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.subplot(2,2,1) plt.plot(ts[:-1],u_l2[0,:]) plt.plot(ts[:-1],u_li[0,:], '--') plt.xlabel('time') plt.ylabel(r'$u_1$') plt.grid() plt.subplot(2,2,2) plt.plot(ts[:-1],u_l2[1,:]) plt.plot(ts[:-1],u_li[1,:], '--') plt.xlabel('time') plt.ylabel(r'$u_2$') plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.plot(ts[:-1],np.linalg.norm(u_l2,2,axis=0), label=r'$\ell_2$ optimal') plt.plot(ts[:-1],np.linalg.norm(u_l1,2,axis=0), label=r'$\ell_1$ optimal') plt.plot(ts[:-1],np.linalg.norm(u_li,2,axis=0), label=r'$\ell_\infty$ optimal') plt.xlabel('time') plt.ylabel(r'\|u_t\|') plt.legend() plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.plot(x_l2[0,:],x_l2[1,:], label=r'Optimal trajectory ($\ell_2$ optimal)', alpha=0.5) plt.plot(x_li[0,:],x_li[1,:], '--', label=r'Optimal trajectory ($\ell_\infty$ optimal)') plt.plot(x_0[0], x_0[1], 'o', markersize=7, label='Initial position') plt.plot(x_des[0], x_des[1], '*', markersize=10, label='Target position') plt.broken_barh([(p_lb[0], p_ub[0])], (p_lb[1], p_ub[1]-p_lb[1]), \ alpha = 0.1, label='Feasible region') plt.title('Trajectory') plt.legend() for i in range(0,N-1,10): plt.arrow(x_l2[0,i], x_l2[1,i], 10*u_l2[0,i], 10*u_l2[1,i], head_width=0.2, width=0.2, fc='tab:red', ec='none', alpha=0.5) plt.arrow(x_li[0,i], x_li[1,i], 10*u_li[0,i], 10*u_li[1,i], head_width=0.2, width=0.2, fc='tab:red', ec='none') plt.axis('equal') plt.xlabel(r'$x$ position') plt.ylabel(r'$y$ position') plt.grid() plt.show() N = 1000 # number of timesteps T = 50 # time will vary from 0 to T with step delt ts = np.linspace(0,T,N+1) delt = T/N gamma = .0 # damping, 0 is no damping A1 = np.zeros((2,2)) B1 = np.zeros((2,1)) C1 = np.zeros((1,2)) A1[0,0] = 1 A1[0,1] = (1-gamma*delt/2)*delt A1[1,1] = 1 - gamma*delt B1[0,0] = delt**2/2 B1[1,0] = delt x_01 = np.array([0, 0]) x_des1 = np.array([100, 0]) import cvxpy as cp x = cp.Variable((2,N+1)) # x_{0},...,x_{N} u = cp.Variable((1,N)) # u_{0},...,u_{N-1} obj2 = cp.Minimize(cp.norm(u)) constr = [ x[:,-1] == x_des1, x[:,0] == x_01 ] for t in range(N): constr += [ x[:,t+1] == A1@x[:,t] + B1@u[:,t] ] cp.Problem(obj2, constr).solve() x_cp2 = np.array(x.value) u_cp2 = np.array(u.value) obj1 = cp.Minimize(cp.norm1(u)) cp.Problem(obj1, constr).solve() x_cp1 = np.array(x.value) u_cp1 = np.array(u.value) obji = cp.Minimize(cp.norm_inf(u)) cp.Problem(obji, constr).solve() x_cpi = np.array(x.value) u_cpi = np.array(u.value) plt.figure(figsize=(14,9), dpi=100) plt.subplot(2,1,1) plt.plot(ts,x_cp2[0,:], label=r'$\ell_2$ optimal') plt.plot(ts,x_cp1[0,:], label=r'$\ell_1$ optimal') plt.plot(ts,x_cpi[0,:], label=r'$\ell_\infty$ optimal') plt.xlabel('time') plt.ylabel('x position') plt.legend() plt.grid() plt.subplot(2,1,2) plt.plot(ts,x_cp2[1,:], label=r'$\ell_2$ optimal') plt.plot(ts,x_cp1[1,:], label=r'$\ell_1$ optimal') plt.plot(ts,x_cpi[1,:], label=r'$\ell_\infty$ optimal') plt.xlabel('time') plt.ylabel('x velocity') plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.subplot(2,1,1) plt.plot(ts[:-1],u_cp2[0,:], label=r'$\ell_2$ optimal') plt.plot(ts[:-1],u_cp1[0,:], label=r'$\ell_1$ optimal') plt.plot(ts[:-1],u_cpi[0,:], label=r'$\ell_\infty$ optimal') plt.xlabel('time') plt.ylabel(r'$u_1$') plt.grid() plt.show() plt.figure(figsize=(14,9), dpi=100) plt.plot(ts[:-1],u_cp2[0,:], label=r'$\ell_2$ optimal') plt.plot(ts[:-1],u_cp1[0,:], label=r'$\ell_1$ optimal') plt.plot(ts[:-1],u_cpi[0,:], label=r'$\ell_\infty$ optimal') plt.xlabel('time') plt.ylabel(r'$u_1$') plt.legend() plt.ylim(-1, 1) plt.grid() plt.show()