npc2

npc2.py

+import numpy as np
+import casadi as cas
+import time, math
+import matplotlib.pyplot as plt
+
+# Double integrator dynamics system data
+
+# State-space model: x+ = Ax + Bu ; y = Cx + Du
+h = 0.5
+A = np.block([[np.eye(2), h * np.eye(2)],
+              [np.zeros((2, 2)), np.eye(2)]])
+B = np.vstack([np.zeros((2, 2)), h * np.eye(2)])
+C = np.hstack([np.eye(2), np.zeros((2, 2))])
+D = np.zeros((2, 2))
+
+# Model dimensions
+dx, du = np.shape(B)
+dy = np.shape(C)[0]
+
+# Initial conditions
+x0 = np.array([0.5, 1, 0, 0])
+u0 = np.zeros((du, 1))
+
+
+# Constraints
+umin = -0.25
+umax = 0.25
+delta_umin = -0.1
+delta_umax = 0.1
+ymin = -10
+ymax = 10
+
+# Define control parameters
+Q = np.block([[np.eye(2), np.zeros((2, 2))],
+              [np.zeros((2, 2)), 1*np.eye(2)]]) # cost for the state x
+R = 1 # cost for the input u
+Qy = np.eye(dy)  # cost for the output y
+P = 1 * np.eye(dx)  # terminal cost
+
+# Number of predictions and simulations
+Npred = 5
+Nsim = 100
+
+# Final point
+# xref =  = np.array([0.5, 0.5, 0, 0])
+omega = 2*math.pi*0.01
+l = Nsim+1+Npred
+xref_array = np.array([[math.sin(x * omega) for x in range(l)], 
+                 [math.cos(x * omega) for x in range(l)],  
+                 [0 for x in range(l)],
+                 [0 for x in range(l)]
+                 ])
+
+
+# Optimization problem using CasADi
+solver = cas.Opti()  # create an Opti object
+
+# Define variables
+x = solver.variable(dx, Npred + 1)
+u = solver.variable(du, Npred)
+xref = solver.parameter(dx, Npred+1)
+x_init = solver.parameter(dx, 1)
+u_init = solver.parameter(du, 1)
+
+# Initialize constraints
+solver.subject_to(x[:, 0] == x_init)
+for k in range(0, Npred):
+    solver.subject_to(x[:, k + 1] == cas.mtimes(A, x[:, k]) + cas.mtimes(B, u[:, k]))  # dynamics
+    solver.subject_to(umin <= u[:, k])  # input magnitude constraints
+    solver.subject_to(u[:, k] <= umax)
+    solver.subject_to(ymin <= cas.mtimes(C, x[:, k]) + cas.mtimes(D, u[:, k]))  # state magnitude constraints
+    solver.subject_to(cas.mtimes(C, x[:, k]) + cas.mtimes(D, u[:, k]) <= ymax)
+
+    if k == 0:
+        solver.subject_to(delta_umin <= u[:, k] - u_init)
+        solver.subject_to(u[:, k] - u_init <= delta_umax)
+    else:
+        solver.subject_to(delta_umin <= u[:, k] - u[:, k - 1])
+        solver.subject_to(u[:, k] - u[:, k - 1] <= delta_umax)
+
+# Initialize objective
+objective = 0
+for k in range(0, Npred):
+    objective = objective + cas.mtimes(cas.mtimes(cas.transpose(x[:, k] - xref[:, k]), Q), x[:, k] - xref[:, k]) + \
+                cas.mtimes(cas.mtimes(cas.transpose(u[:, k]), R), u[:, k])  # quadratic cost function
+objective = objective + cas.mtimes(cas.mtimes(cas.transpose(x[:, Npred] - xref[:, Npred]), P), x[:, Npred] - xref[:, Npred])
+solver.minimize(objective)
+
+# Define the solver
+options = {'ipopt': {'print_level': 0, 'sb': 'yes'}, 'print_time': 0}
+solver.solver('ipopt', options)
+
+# Simulation loop
+usim = np.zeros((du, Nsim))
+ysim = np.zeros((dy, Nsim))
+xsim = np.zeros((dx, Nsim + 1))
+xsim[:, 0] = x0
+usim_init = u0
+
+
+timer_start = time.time()
+for i in range(Nsim):
+    solver.set_value(x_init, xsim[:, i])
+    solver.set_value(u_init, usim_init)
+    solver.set_value(xref, xref_array[:, i:i+Npred+1])
+    sol = solver.solve()
+    u_sol = sol.value(u)
+    usim_init = u_sol[:, 0]
+    usim[:, i] = u_sol[:, 0]
+    xsim[:, i + 1] = A @ xsim[:, i] + B @ usim[:, i] # update the dynamics
+    ysim[:, i] = C @ xsim[:, i] + D @ usim[:, i] # update the output
+
+
+timer_end = time.time()
+time_elapsed = timer_end - timer_start
+print(f'Total time: {time_elapsed} (s)')
+
+# Plot the results
+plt.figure()
+plt.stem(ysim[0, :], label='y1')
+plt.stem(ysim[1, :], label='y2', linefmt='r-', markerfmt='ro')
+plt.title('Output y')
+plt.grid()
+plt.legend()
+plt.show()
+
+plt.figure()
+plt.scatter(xsim[0, :], xsim[1, :], edgecolors='red', facecolors='none')
+plt.title('State space')
+plt.grid()
+plt.xlabel('x1')
+plt.ylabel('x2')
+plt.show()
+
+plt.figure()
+plt.stem(usim[0, :], label='u1')
+plt.stem(usim[1, :], label='u2', linefmt='r-', markerfmt='ro')
+plt.title('Input u')
+plt.grid()
+plt.legend()
+plt.show()
+
+# Analyze the results
+error = np.sqrt(np.sum(xsim ** 2, axis=0))
+avg_error = np.mean(error)
+print(f'Average error: {avg_error}')
+
+avg_u = np.mean(np.abs(usim), axis=1)
+print(f'Average input: {avg_u}')