From fc7941ad2679431476c9f0923cec54873d060772 Mon Sep 17 00:00:00 2001 From: mcarfagno Date: Tue, 30 Jun 2020 17:21:27 +0100 Subject: [PATCH] WIP: racecar mpc --- .old/tracking/mpc_demo/cvxpy_mpc.py | 178 +++ .../tracking/mpc_demo}/data/checker_blue.png | Bin .../tracking/mpc_demo}/data/plane.mtl | 0 .../tracking/mpc_demo}/data/plane.obj | 0 .../tracking/mpc_demo}/data/plane.urdf | 0 .../tracking/mpc_demo}/data/turtlebot.urdf | 0 .../meshes/images/main_body.jpg | Bin .../meshes/images/wheel.jpg | Bin .../kobuki_description/meshes/main_body.dae | 0 .../kobuki_description/meshes/wheel.dae | 0 .../meshes/sensors/asus_xtion_pro_live.dae | 0 .../meshes/sensors/asus_xtion_pro_live.png | Bin .../meshes/sensors/kinect.dae | 0 .../meshes/sensors/kinect.jpg | Bin .../meshes/sensors/kinect.tga | Bin .../meshes/stacks/hexagons/_DS_Store | Bin .../meshes/stacks/hexagons/images/1f_pole.jpg | Bin .../stacks/hexagons/images/1f_stack.jpg | Bin .../meshes/stacks/hexagons/images/2f_pole.jpg | Bin .../stacks/hexagons/images/2f_stack.jpg | Bin .../meshes/stacks/hexagons/images/3f_pole.jpg | Bin .../stacks/hexagons/images/3f_stack.jpg | Bin .../stacks/hexagons/images/3f_stack1.jpg | Bin .../stacks/hexagons/images/kinect_pole.jpg | Bin .../hexagons/images/kinect_pole_old.jpg | Bin .../meshes/stacks/hexagons/plate_bottom.dae | 0 .../meshes/stacks/hexagons/plate_middle.dae | 0 .../meshes/stacks/hexagons/plate_top.dae | 0 .../meshes/stacks/hexagons/pole_bottom.dae | 0 .../meshes/stacks/hexagons/pole_kinect.dae | 0 .../meshes/stacks/hexagons/pole_middle.dae | 0 .../meshes/stacks/hexagons/pole_top.dae | 0 .../tracking/mpc_demo}/mpc_demo_nosim.py | 36 +- .../tracking/mpc_demo}/mpc_demo_pybullet.py | 0 .old/tracking/mpc_demo/utils.py | 33 + .../notebooks}/MPC_tracking_cvxpy.ipynb | 0 .../notebooks/img/mpc_block_diagram.png | Bin 0 -> 39437 bytes .old/tracking/notebooks/img/mpc_t.png | Bin 0 -> 32576 bytes .../.ipynb_checkpoints/main-checkpoint.py | 184 --- mpc_demo/__pycache__/cvxpy_mpc.cpython-35.pyc | Bin 4325 -> 0 bytes mpc_demo/__pycache__/cvxpy_mpc.cpython-36.pyc | Bin 3787 -> 0 bytes mpc_demo/__pycache__/utils.cpython-35.pyc | Bin 1332 -> 0 bytes mpc_demo/__pycache__/utils.cpython-36.pyc | Bin 1217 -> 0 bytes mpc_demo/cvxpy_mpc.py | 142 +- {mpc_demo_v2 => mpc_demo}/mpc_config.py | 0 mpc_demo/mpc_demo_nosim.py | 36 +- mpc_demo/mpc_demo_pybullet.py | 25 +- mpc_demo/utils.py | 75 +- mpc_demo_v2/cvxpy_mpc.py | 104 -- mpc_demo_v2/utils.py | 84 -- notebooks/MPC_cte_cvxpy.ipynb | 18 +- notebooks/MPC_racecar.ipynb | 1179 +++++++++++++++++ notebooks/equations.ipynb | 214 ++- racecar.py | 52 + 54 files changed, 1755 insertions(+), 605 deletions(-) create mode 100755 .old/tracking/mpc_demo/cvxpy_mpc.py rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/checker_blue.png (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/plane.mtl (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/plane.obj (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/plane.urdf (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot.urdf (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/kobuki_description/meshes/images/main_body.jpg (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/kobuki_description/meshes/images/wheel.jpg (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/kobuki_description/meshes/main_body.dae (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/kobuki_description/meshes/wheel.dae (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/sensors/asus_xtion_pro_live.dae (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/sensors/asus_xtion_pro_live.png (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/sensors/kinect.dae (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/sensors/kinect.jpg (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/sensors/kinect.tga (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/_DS_Store (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/1f_pole.jpg (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/1f_stack.jpg (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/2f_pole.jpg (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/2f_stack.jpg (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_pole.jpg (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_stack.jpg (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/3f_stack1.jpg (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/kinect_pole.jpg (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/images/kinect_pole_old.jpg (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_bottom.dae (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_middle.dae (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/plate_top.dae (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_bottom.dae (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_kinect.dae (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_middle.dae (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/data/turtlebot/turtlebot_description/meshes/stacks/hexagons/pole_top.dae (100%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/mpc_demo_nosim.py (89%) rename {mpc_demo_v2 => .old/tracking/mpc_demo}/mpc_demo_pybullet.py (100%) create mode 100755 .old/tracking/mpc_demo/utils.py rename {notebooks => .old/tracking/notebooks}/MPC_tracking_cvxpy.ipynb (100%) create mode 100644 .old/tracking/notebooks/img/mpc_block_diagram.png create mode 100644 .old/tracking/notebooks/img/mpc_t.png delete mode 100755 mpc_demo/.ipynb_checkpoints/main-checkpoint.py delete mode 100644 mpc_demo/__pycache__/cvxpy_mpc.cpython-35.pyc delete mode 100644 mpc_demo/__pycache__/cvxpy_mpc.cpython-36.pyc delete mode 100644 mpc_demo/__pycache__/utils.cpython-35.pyc delete mode 100644 mpc_demo/__pycache__/utils.cpython-36.pyc rename {mpc_demo_v2 => mpc_demo}/mpc_config.py (100%) delete mode 100755 mpc_demo_v2/cvxpy_mpc.py delete mode 100755 mpc_demo_v2/utils.py create mode 100644 notebooks/MPC_racecar.ipynb create mode 100644 racecar.py 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 0000000000000000000000000000000000000000..7f322d12e6d1799fa42e6eb2c90511e3d2259730 GIT binary patch literal 39437 zcmeFZc|6qZ`UgHyX;)7vRPih;i7)=m7IP$<+^ ztd_bF3bj50g<4a+VLg1pu)LaxLT&I1&@d11rnc4lJX`JXd(1zDVdf3MA34RmaE4* z5L}3VUQ3S@?C#~|AS8*A78Qp}9Wj@X5JIcMD^o`@F@U`KTGyROrov}bIT}#Dkb_ol>=NKWSRJIBrQ$qhMB#33T!EcP9~750j7-MT??WzX%BOaasM;*(K23$z}CZA9xu5KMzJe zL2JOIIlDU${2aVt6z+dcijyu`oE$(wyW;e`b)`J8Zr(w9s}~?Tc>j4>1Gv-)7&>wh zS+f93Cm~5ytdxd_H(lZQp zmhdo_pavQH8U<)lWHn4FSUFE)4-*45984WaUroorM_=0CUES0{!;R|fPSjDO=$IQ( zG{kjGF`CAwHFTx?^r-=az(5aE4Gjr`QJ`+%X+LpaI2kJ?kJFGOI!buxN@K;1O?=IR zg8fL|4jSgC;ahEji2?Fk69(<(jd37*n_C)^HS~-zlJ2f1h8Qg`Sx+OJ1Hr=80!#HU zlJ<6&kQK+dNP6Kdb>S2O+FU}%(L>uqjX*{lohHefx>~rnO3S$h>iSVU-~jk>b@z5K zhTp+HJ{YhLRYMc_PfO3ohl<5{=}3AKd`RZf1UI~ldyu!goVvJ%uCKhMo;radX=#FU z#W)&E8so4OejAg-;vkcoR-* znd^F5sHqv6>IS&tOf}$`0BYcAtehd$*xy@Q-{`cn79K+)nBw6KT{(B%VBa7!4RL9z zu8$!FtF0knOttiskhIXF$f(L{2AEq=9LcBUomAC=#r-|BWIbJtNF;AJZ8fUCwzpP* zn;aofg6ynkfTh5V0`#f{*)?ih1;3on8!WhBs&R3jN8PTj##hvG@XpEhyz(F*iY z_oG?_SO&-vJdwljCIl}TjAalE3gd4<^uU>Gx#@ejA#W`Mf=N0O6q2@~k<4kFjsYo9 z-BioVED)z*h7EQ+jZv5ORx^?J!8`b1-Q4A6q%FPhrrt&*BU3XUys@u4L4!i@Gj`W8 zw6p|!QbnsfNt&3G_5EFRW#Fx7(sOtf?)Egkfooy-jIrV=t*eo`9xPC-T%(w4rMKn+cCD%!(Q(o0rd$HS7SF6VjL zgy84qLC{t6fH}cPYZ3gkEab=(Sp$kJRaVDc)5+XX#?Tc{IF0e~HZu?MFj1wrQzc}A z)x@zHUXBv_5)@N!Ka!8E2hPn>Llq~j5iBJy7w8ut<4W{F%V|2Ibu=9HbhWYC1~O`a zuI{=fx~}SS7J8D7I$D7S#&|tL2^l?tyQD8MKojk(>1V0t2hS@b>0oI>#Cpl8s>#U( z!nm9;uFg`L>T+({jvj{k7*lm|SM@+cKV2UKypJxy-`7%Hp6cT5Ac_+U68}F9$6RBMQdfm=qXfp`#saBJbgEXklz@;p<`Khu0x_ zI#Ue{#kDQGh(wZ;w39`E7m@1cgQ56KI~w^qn~*WVrjABL=YU|1V2qceW`HBr$5CF* zNK=BOW}xkq#k$1p4!zV$O@=ka|c|%`!Z=#u>qk%r|v<29yys^K(oVdFT z)mWBFA{(3dNV&KYh{hT&1UIi>ZM1}5ke|1sE84|ELf!(dLd21A2D09o0nYw1;OsCS zCbE*=zIbq(|JGjAP%TdrqzJXGP(rSKwaDtJkD^(V&Yw1RI)kK?6Bm&Ups$`sox4fK_zrM5- zMn{8y_I0Cr`^$RClidCBx{{7Eo}M^OZeVpv7=q?Ry!}TW+zyQ>=doUreg5?K3kq^lxvw0b=8vM zX_Kr-K0$2EZN6&cZIj9&df+4f#*G8)FhQq-Z8r}eelJ@&5u}=#@A}7kXIHoCw zoz?=j)zh0x%_J!~G>rL5qP5Zyk5&!VNQ~sKm z|1e`5j_;rEpPU{Zd@1D>VR3H+r0Q_4Yy%3lGX`&xekZ9W_4#vsZ||`j%Z%Xp zoYMnh)kCi5{=f~Q+|F|Q76(MO@ezV>?B@7lqaz^$TfLZh8CRGFEQ z7Z@Sz+BF1+D+O7mUEg-iOD5)_m8GevX-0iyhqPaScIk6z<)b<;@J4>nd(X!2Z%@2iR$Wm?~tlZ~gQaleYSDZ|Y-jKkyn|F+InmX%` zr3@%fd`giQ2)V(3Csj{nywNUiJUM$Px1gY)vA#a_61U*Uw|~f<1w;%M9Z}o#$K`!-okO|EWEqOiPB&8)I?L z@CmO2X-%nPicF6m&xZJ=|1iZKZL=(K%&;vv6OuI_&lH$$y@8ekcXsjm(prq~#s@j( z-CD!)eC}S3uJI4&K3q|t{fs4czDe9EEYdk^neF#TqB}k?I%prGeA(t!V|Vv=@`B_a z`cw1s@vw+tKFE58VbwJ%t6X35aV5gtILGDBs22_IwdGSH)Y^>Zh`L+9wzTRvj14Tb z;j@S@9Q(u<3%)_^QdYR(nIGz1#mp?jke_**Ir$mWi$Rzyr~agq&2e%T4vo7*^BYN1 zHA_cP2RgTd3%38Q*GPCT*R#OFuB5a}MHV^JRtW?1o1;%Q69eWX9>Z!7M^<*D{h-g5 z2w28v8aihD_m0~nHuZRay>f@AB1+@-^kTVAd4!(eSE_$fjpV|9m5G?jcKHiBv9k%a za7MXZ#=c)tFX)?48)R3kRNYKJ&i7P%bMts~+;xK0?i1!evt@+Fj1y+RzpqrD=|grX z%HMxW#F-%2eIm+7t2}$&oY(d4p1ea`4AQ}Q4xdyeHCU}|;EWEs8RW?E4uUG7sX3gQ z(w;R>L*s9j|I-s*Cm6~|7tiX{YTgjot3=B$w+RWk^ehgIKBTg5BcrXTn}RAh+62Zt6fDv|>OXNO#J13x)te{{?q{1TJLVEx9cp+0-kbL!WvUJsAp zF+anl1p^rNV5?LfIri;F z6iQrbZehzIJ8+M;yNJ<;$rQ@S*YCX#W39dp$W*ZNg&v8t-I+2iaEQsYE&RSq8EZ73 zl|&nU`%e$I@smZx8g~jM@8XTif=f7Xy;Wr$>ipi7UGU@F_iRkrA*?%} z@%qlGrZh}K&m}>j;Eb%0+1X7AIL8XsNr4r&B`!?o!hHF1LV1joX=^;3SC$;zn-P`g zVxK zF|0n%{8%x7sb3U(=iET-ZScb#54kT^4nts~ZHiFlk4islvDu`+xA4bK;oyCFy*%uW zz|jeX3=w=3@63p{4EI6~II-2ocoxtb*!y_;bJJei5r5?`vcuf_jx(l~vL}rzSetie z{A%@7{b`agJNu}RGj7kS8>TO9i}3J_=VQaB^tX5!Qo9Dc&R%j6a01dl?TKS+mgvkbyL$1rXETYZdD}en~k;xMCv?B=R;pZ0zFLG z;h}C7ZmHwPYDQjF2s!h=*D@+^yJq>?F=NxUtGRP&QO7JwH6V7SUVXWTizdGYwS5N` z@jFAKow>DgM`$ythVuDnR^taD=ivoGk|6KsczTtCp>JwN2+R6u8?ok@?xUM&v$NZ< zw_)2zI5pTbuONZ4uRj=NZjJX%Ac%@o;*A?T$pO=y{P{0m8ehAYZUU}X)TV^k*y<%y z9NaXxOMC`#?Ww7${Z%83!sTP4Z)tPWnfn!)L1PwnJ>r8^8&P4GwN?yL_?SZX(JeV` zUa5=hR2i@KCrzH(xuv6*Ax1|s<4PhMY>W-lE6Ptn@;4N zD*-Ql`SM}JeC}e-#3oPL;X;&RCTe3$iQEtOe-X9*5gsscJUncqrj{UTU6dZAQO+Ko!B$hfA*Py|HhB<uKg!X zM6t^23S5oAD;?i`4Sudi#q&=OA>#-$SOM%|H~)rpO|1ayPIjC5CbO7J?|O3;M%Obi4X)bmOIr#m`iv)T4vbGIHZ~6HjqgRo*v8wO9>c8a9>t~ zLc(J$VSTf~PYssj0P_e!0tCo3aB-3Q9{p$-xChthU3{cKs~K?FK3ug2nJSJ8t8<-* zAS>?Kq+06o6MDl>n*#dst>!4NW?_lmu4bOyJZTvjT~05)c6DXYTrSPL{bim^Q{hvXW1N17fPW*ASbI24y zUdILh6Y|=-X|ag;xWg9Pn-f_u*-Jn$Nbajw9`IxkJ|AP{(I!P+kscny5e1$tOVe^R z0N$TG#)AH6m>Lkv^_U5E(T#n6KYK`fpDY$#YW^7ZCLbGi`9!Ojh%nAVKBzt0KVG8x z`Xg3EX{pGu_K;J)GwqSfv``y<<%70O^kt{%%*>LcLzVieuv`e_S$Z=O?HZb0=ADJ$ z<@Y62!z_|n)DaOyT(5B7@U^yUu|(`#A2t#uA}IEC^5C5El*udm2XzT5;f8X@?U1$F zuy)0~&qpK=Qro;_dM`~qyF0puxlj3E_ZtPwZP$N?{nD@DK1&9-jRi_eNqJa%^oZ#G zxRc|YT_oY*)Qj%a?;Tg-icrAUdJfN6m0wuCAY-O+Lw8nzk!F@9yRdDV4|8doplq;r zTY=T>X|Qn@Tn&RMIbK|!mX=mEw%85GxdLx)(#aV5@^N9Ym?Oc*vIH6FH{9w5Qa0GH5Fz$4Pa z*UNIJSd>=w-Y7fL7%hejW^U)|&c*<)oZviTWmRYwjG*1=!!RupFB~Fih(o)-< zvKTs{QXcqp>w(uq{qMge)DZJVV0tVUo3vNpauEwH?l(Hq)0=dfZ6_D{BQKPj@o%2$ z=d(PONlJkEV*N|V+Un596yAE$yu17pLIb=k!vA% zE}FP-UH52?)nJ!s*gfV*;N+^}Dzg6Zn`CZK6|P`n;Dk~}3OdfSZ&=#=|NWHb`|?zp z0a<&_U7lORd}V$dqhdEKs2uR-pu2UC$ZZauBkJlEM^3QO`<4By^|t*U-J3Sv=AQuw zaH@#wm^i=-ms7MkK&EIYD&)jcgXX+O=AS{c%K=| z;^zS$*J2ojXOQq(7LOol)OI6)mY+SwBtzxmWoVfl?Sr?l`uOen1;-NH-b(heKYtbx zs4V^)UB)S^aBUKd)HZvYzV2Ap8=HwMJC$br&r~$L-XBsFdhc)0?3O<#B0g;MS|;X; z_ZMJUW*F>C&o@MHmIo@`otN6Qw3pq*fhbchz37e2T>Yhkav>FIw>W zPcds^quumx_kwz}%8~(h)d@TOkcyTrWbQM7ZQbHBf=ynF3Ag@dA9NaH-)93EzcH`# z%QlmFmZ^8^E48$iyB_qm9nyKP_!%ld=KuyIvh-q<@sKg!23VKt%@T0YU$9PaYPfq( zLc_DP=h^e;Ett~&o->Ts_ya_X!1>RDm@eQUAfY^nAnVrbA^vCu3$*L*Az(|uJ7epM z=>l%%Cb2EG5~9C1@M3NstFdj&;qf#zH%W(_*W}Z}2?gpL-ZZv&FgLoCBS9&Io1d0S zT+Vpr+g1?*i5)q6@+ttq$;na^JifL`>%@r+1T^H>fyes%`P0bziO)Gz(OHG*zT|S8 z+=+kkLb??mxv8aM*UBXa8}?t_CU8vf*NxuHyTsN%RyP>{%Jnp5wWiBAcA74YxMwYw z(!0}*0IIfxzT6xwXOsJ5*d^DpSd?*U?WXt-5qMM6T-?Y=4`tcZ`s~QB4^e*s@Nc;{ zVBmm=X8PPS8s@;~r6s&Sh#ejsZG0*u!UOIGT+d!<;G42+r4`ocf^%1<6Ekfyhndy_h$Pp2 zN)$mh`S}`fZ2(RStnt$J`G(MPVV9t>9hXo2elTYsUkP%lp}eb`gDs7P;kg0!U+a(b z7>P^!;gxuG_i=}ecn`2yTy@YHkTnXOp?Negj+TkCOJ;S`MbRw7^3Z} zA#$(K8WB$RuY&bb%ra~lQAr)7t)_K{?nGMM|BJSMc?iMv&ignI#f48Sx$fFEKjwjm z1O9@|VV~yE;*-pb2M`d^9x~qDiGZdeuj@5HiiqTL2v=8AqmPi2Y!}T=<-YDH*!T;s zrdL8s1N3HQwnEAS%o75J0U;g{Th_@^dir$d0!=6FRm4$&kmoMC6LO0w4FlbtZ4v*4 z34}TR*bgq}S7zG<9h1<-h;bF_v)8W!dz)ix?TQ-vt6Fm5NuO%|F_T7k949{{_C%S6 zLdhZiwai&nUR;|`r7gJ))(cq0l#-IS-uLTUTc6#XxKpe@&^Y8j^FyP!#b;nNUpsP6 zuFgB&XJZ6prAntc=^4M8LlruErfuM>9JW@2_Fk9vR`4CoNqUd0K%+Ynq7 z%%bzCP7xqOfQZR2lFb_k)eS(q=emp#Asu#WeD&(pF@)}77|?$@C*uS`DDwB5`i&Cu zI-0(IP4-QpWtm^WeTKxlA~`a$)~AgD!87>RFO`HINZz8!PCx!gem$OFN1DKk6xXnT zj*Sea3w|Rw)0S+*>wfK+NF`BSwkU>-!?A#$n}Gj0BMLksmrD>#c+tp6e-9sPJB7hc zyMYlZOur`ZAU$OHq__vSp!|3)C0WH^vN4Gp6-GfU+g|efZ@k$0cjZ8BDo%B*%X+Kz zJz6L7VFRHiB(}swq*;47-?XF6KgxDMX%}aLkWQ)eMMcOaE88L0}G@HS951a;);si3}A2)8yXIsiZhKpeOag!d%dp&CxW%35! zzF!WQY_#C1wv`)PRzZb*Tb=B8LgJA60!N@iY;$Q81)x~<5P&|vwEw1PYHSIN%Ok++ zVQoAXLN#nP-uFEJ!eE}Z5P%0_6rAbjb`m@&OnKh2Af$soN*O+`aU*~xWORVz0T3zr zZ|@bhT-!7A@TPo|Q`V_6O^ws-PJ(H zC#rg-Dsu@Z$!gysVvAV*H)Sy zklg-+mseUL)|8*;(YM_0iF`eX(i|lxVN$yRC|$|su4``2d?yyYkddb{+w$~jFL(~o z6yXT0&T_R&S4OIbll28+TnyjkA(lNgU4YG0H>$;+TK97In{*wmn))=n(vw@|j&-dc zbn}!b40(Iq=)(EP%Mo=X9;Lj)uPrl(S#uny`$#5=~GZ$o?}; zI&_>P?Dz8sUcR$2n!r1s5I@BuyHuPP40c#dh}5qCSAv%746>cyNFG0?Ur1jnf0OaB zx5^`T_@`Z-DS*@4#{LUUvNu_tjE8@vJEvnx&RwEQ2ftDLl-BaG?kKp(+s|U?U4d2u zbJn_a+lh6k4fK^zS8cxz161-X|B*Sx$8!TcTo^q%-Tv1fyEqS35@hBjON;S`Ei!Bt z?5SR5(Od{$lkL=yo9XUo^w9W?dIN!~tcX4+DaDxpI4R=$?&Fr< zvjhL4fme5L3IBxe)5*<0Jy|uy`gwIx!Et0MC)Zmo0!y8C*8uANJfSjF5uZ2x>Uj+{ z^Hv&OoHL=loj^=_-%-`TGiQ~zoIXB2uKfPwKXNcpoE#X{-c_TjEjt2PoZQXykl9Y} zE|_Yl+4A;oHM8%d;+Q~11dZZ>y{OIIajS+IW{1yd%c>fraV7wd81Wr;B{jXPfK5o4 z1uT9xXJudDfh+D>9C@8xYG~KW$v(a=%c4+kS4vU{KDl~1Ksil< zGMui=;0A~flrj*q7@HRy3EhiFU2mu)(+)MDjW~^)@fAu)1mA~ z9{lC22k>$FqjOTh+XA63<4d>M5P-vuy-i4SwjNcR}#jMPs=?u>GZR0@KS|QKx)X zxKT-Sv>8k+^i0nlnVt07`SW|4qD@$M+89C=GSz;P8Y691G}UI!T} z*f4q%;0g5}DdUAk@&@~>)+e+;C{q4H0qVkdgPHH_(|Z}_X;7LxvE-4c-At-``@S^w z$EZf0l2N^~shPg5;C1{ao02nKE_zU!i&jd~i}zeicKCNGNMFFcK_I6oinNs}XiSF+ zLw4;%Rk)LYCws?7>%!26!EAsEdF1#39)2>cIs1MsV$B<# zCE8^*10guQIIjb6iX|1%>wTVEz^_zg*N&n1Q_u7B%LgZbd(I<6R<<(i*-<68Y4g{P z-UiVc%h0swxCxW+I1Z2 zDY7I5)1F`2lvKu$`TS9u>0(BQ11ArZ^(qGM+@ENF6%-BdqN%gnJ0o#V4tXKgorA~AXL z##+&X%Q#e68M2DVzp$RjD>08jn6N`(wA=p_P)%g3(&>?=ZF${eM)hyJZm`)C)!51b zva4?F&m31~9H%~>f?o=4O6pz;{^KWPchU=s1_eznNOn*DW}k1!>|*Y)uEWl1YaPK(VtgEXF^Xu1W+h&jjoYE<^&47vmQUibO)s>;x z#O#{`W}(ZIL_g_YKXH$Q)UP(Ba zsw?Y)%oB&}+owFi>;j9DA|M@JEfj85W(o|Vg*79wO?Q%V^=I_-vLZzifS1a3`=pJm zd42x~Ci07GLmEEPHtQ_^;Pec_AxDetEt)Oxo4JSyq3#BSS#3rnt(ZKynk6%RwxH~{ zA^Q~hw;}n43SUxGns&hVA1+8$2uv23hOx2zsUfr0P^5U}I<}tnLuk~;Q+E1A7M$-p&JG=V2y+EFx~UaQH+zC_O?-(hh)cjD zn}Ep~Kv^ffSrjpHP{e>cdT40SH$|8Q)*hhf^PXd(gYJ5dj^B&xcgQX>1gg+9Hp2v| zPuWvPA1^adsI!08M3AxDU;fL$;0^DO6^aS@9P|uwtJN0db=dFMSBN^~^?yBOdroKZ z`j7$?wQ^xIO8n4k5(IQRbL+4iz=&L0Zbo%gzej4mtG1-IkMKH z#L>uYx97+m;Ci=bL=06H(OLq&Ap*Jo`QzI@hW{%6-(~+d3IDT%|80l=`Go&Jjxh`f zMTF8aUmn;cI@Iz4p*OaK3{U5uj^Bgk{Wux+?;J0pN7y927PGXX7**ZP)pu&nkv-#O zYnZJcr;r9&E~sOWyb_=>=wPrHG|JB@(|GTnyYyKI3)#QmwELQGc3wT=cfDF)y&qdH zIt+#DX+RRl!9D5645vT#osR!$&!=_D@LwebK|?$>EV2K4XYvUUzacPcqH+(mz_<4v z6yUcmO#fgnhAca@E`8%DL%$~RY8S1;FiQCM#z=*X|2Q2#ZZGb1Y6o1+)mq@(3L*Vh zAnCvZDhHyXe(pE`g8!f!rD{D&@!Sn`u1j9W`&q6%sJLv}dxIamydckcnsv2QdKZ9} z+z}ZBOiIh<@se$9Y#<~{4K@h4xo$r4j<#IxRplg(@MU2)%h$+nFEOXHQqzkZ&ujyk zg|^OeI!+Ig45bCe6+_B5`}xN>*#!a!T(cM7P5jf{khSyf(oDEbp0!RiWW9Q>54w~6 z(?i^cU#{Ery=2+(b-%>@e~G9y%Ix&Axh)_Esz)akWaw#sG6T)Wv$&T5cOd<5{2=VY z#|AylxNj#A9Bz%7=J_?=HjvmD;sd`WPcR{|dgb4utQ0(LPU;R&I8h5IpDQE2_m|3D zViTm=DJiQe($4x+7E{n25U2K|n+Lt2nz?7Upp~Q1U=QD1?dBL|W&zG%1CQ2L=VR)>QVe zfksG!iR$<+1OkNmh58Hu*0YFsuSN!B6gfFlv7vpz%%)X(ylWO<(!0GKvSqm-C538= zLh$G_!{sr4CO+l-@X~3}n;<%hr|(U_GfEH@EEIAPnbSm`Jr~9}skR9SMFEy}J@Bmv zfzhEH-cH}x-@k_O?ho-{hKdi263QwgpmF~4h-qpddJd!own8di%^*pE>^tG-&)xm{ z^NwFt6j5hm*9wtTm1P4r9W z;{By-kBK9>_p`WjbKejBFl94UDqb`-F$4Nqu~GZwQi0oP5R_f2uNK?AHsen&(Z9kM zJQ>sAf;aUnO@=N=Z{>1nq1#&eEpj|7QMf+c5wb$G?ZVOo_{HnlFvH@ zSzt1?jUOQx$Ss-dAz#~c;zTHUbld+ar>d4CIvTa)Eba(5556F0w{BcbzbI z-O|xhb_7ZVfV#1u)@+&BS(*YAzI^;oky87T&?8;L0l!r@dCH!gonkKOB#UuV3GHQr zT`gZqiJrQh=fm`S#kj7|@QEpMoEqGmTXqBluijltFJBXvEkJH^ZfPDB{_PEVPcm`> z_re(t*AELFbi=v{AXq9EVKKPh5TJ&ycIn36T%g|W9Y$1MP(B^`JwCpRh=-h^B5}6(>=DqgLmf6jMn?Jl$g51t;_hVb zN&={dc{*etXDzMA4Ad+W8^tUNK6~3Fj|)cZd*tZF>;VZ<2zdfh4gHZoI_K4h^x8-V z)=plU_(?Nvfh9sU>1fFZ#y+ZQh2a*y~YO#k_ z8e#{+|E;Z27{SoHN1%3x^&m06cGE7sa{Kf-?2-v;eT~El4L`^Jq5ULg>og0|oI3@B zq@e3DUHR03wY|sn_1jS#=VtPjD-uhJ*$=vi%qj^iOp;*UsH8_>K>?I~lIlc>R|Z7r zGa`$ADRl!M>vjrw;xms)c<9IynG5s(RJlkX&UTjDCPkHP*nsou2`gFH$y*M>NtJOB zL4=%HvV*w*h09n+Xr+E|Y)f}fc;dHpnCnv^3)Mk4a;wbJrSFv^WsVS*)3T6tG*|2e z7klSy^E3R2a1BZJ8Y+U`9%OQHaDIRD)Ia3dfg)&c$jz-?PYd19EP$?hga^KuM1Yl37 zLwAH;SMfLj)ruB(A@Cx5z>7TiKZ0-p*2Uqa@ZPS3qAt}rMj>fH);zp!$0dS#&|X5{ zB`0YGnJ>M{*ALN0OV5u095Cj>6lX5`P8FNITk<)%q4R}%I|s-wp>3ISN^Fj z=9GQ*(kyPg(V}8edPQUnBfhS(dyzRe6qeLr$4*US@X`E|YK)AuUuza~HxS+3XBOr` zkw4opz#KohlXuPc3uAk~RZEB?b%EU{EZog=0;0_5d8eVWBG*+i1N$C|2T&M<`L`}y znw9MX+f%yEY5~rr9cXB#;7Q4gKAVr;mfQPVc-=5VhWCAUiNEZD1{m#m zF^)s_zt;X(nd7tPW?Co@?sP95`C3<3>cmGmyMq@>Dazl5rtzhah%?yq{^9S{;R}o1 zj|d!x4lKT)ad!Lttw#S8a&t#2i;FqlCEI;N$A_*{jKHcc%Ex5jZyWc%&@zIVWEZ5f zvzLF%tm3S>s1B^|@`vH!5aI}^Q`3NRHi4ubwjk)$hM}+<08|VTp7*(aY0q=Gn--mh z*6JplECvcn2n(Ar^YX{Ij`6W}o9E!Igs@O2;*>1uK9>w6D$-0qggQBdm*e+j3zom# z`b<|En|x z8uw+E2yeI{C>=QC^yxDrH1SDR5dgAvV^blNxQ!8EzuwPorB+$NPAD=2Mj7dZ_*ujH zJW%qafc|T+RJei_PYrrrpV{St@qWQ^2nw?)1bTf>Rl6Wd?0dndM|g;(!QR=supPv-Bf6~=zo3V zi|f=}MDmq8&k=djjfTl@-|3(R{^3WZS95>c6mIBKf%gafp6?|I#oMc>X~&v;ch!4Go{$vSzIy_{vLJwc^8B5KZ1NYdI6_KwVMU z4jl-Qh*d@c#q{0U<`%lO>M-IAwq7Y4Y|mARbl7vkVraS>dS;Tge*n<@uDl8r+~zvL436!u5qe@ z0%{ec-iWO)WeR$9TvtLgX5O8z)N;urwa1{+j)qQ)_?u}^!cQ279fc67fc_H`K)#rt z*fdlO3?z#zzb5Xyw*4-&j18c%X{ZJEvXLTfk8pAHUY4Txre?5&>ngP?mhc8FVSwsb z@qa{AGs3@GbI5sydo#o9pv{PvrYi5V`2%6YfjQuClvI^K;IQ;IQl)2U_LlsoDYQTI zj7fN$BQprT9{br}jmCudWk_>{c0rR?K`_Na^UdSV<9 zslP6c?cKf*eyfzpeSM#j!*mSv3skm)Qh`7B?WPFWvff+pAO)>Nlw3%K3hK;m3tJd( zb5-tP5r?+9s_did%OQk-4^;}qv@E|F3TXO_ps8eu6n!O?#UqAUcCtFRZA-kqx^XjA zV;AQ40qu&dzauP7>t^oao%cUktny1+NyV$MU{Tzlw~)dpQRWpG$+^+X-5J6Z3=9?n zy-1S4_;VL8$LA~E17NM3k}6>$*TW6zw$L7wf_t+b^L9ab&B=*Mi`dxWja#-g+x2`9 z*1y}-?ovJVSC>OQo9jk*sIus`$%Lmn8@swrV1WVc^v;4*qOopbHjw(ApI0w_Vs?2H zS}p`U6Q1_w0@6lGkYiI%Irw-p0^V4jvHA4#^l}g9uTU_WCxX25Ac?_3@CKMn>vEvjg!Y zS9$yt=Nnb~U9pUvWijEgNYpF)1}X5D8XH!`eCWS+O2|eQeD29A7{gn!s*|f8DWui1 z-aq|e0%am!qkFqb!VLk!H`r_q^?7MG33M&Q9bzMm$;L#J!?#4N$lv_aY!4J6`Q@Hr zIQki#$qkKkdnpzm@A>?3IArfDFp>gTKi_xhyNb#X0U8L-LB9@C&F_vawYP6Tsh$7Q zz(!Ct4CdyQ0tE97Dr>vBA>vuwrGnBJGllL<;+OWV$do5Q%D^{cS8dzZSY>SAS34oM zRJIpPxu3u@IswGE8^|0CnM8X;8QfdkO;DJ*q%y$eQx3okuo~0~X@0+cW$Dx2$%}8Q z0O#=K)JmAJ_sL#b_~SpcYc$yWh6vre2i3K%+bAA^LkxHBv{y$aX%1f? zA-jeAeLL)gGJmL%XPNHXr$`eg`Tu zzW31V_jd(PKVBrQ3EC>veE%_-0 zcD#3e={|ooS3mEvZSnI)r&|C zcZ5+8-sXZDl;eg<+2;UVaU^uNATGtTjpZMT!e0!w-<1yga1XN<7uhPb+pnLegRT689oY}i?upvHOy*w%}WBvb8X35oj zj7(LWRUy=4jlqPfK-K*vMj!ef>Y*)I!7NnUj)$%|xO@p!uVN~mt3mwQZ=+>ykoQ!~f$t6SxO@df%&UJFU@z`P zDGp^fGq8f0RX6`#h`D(6_Z__Q-IEUOfEauBGr~bbU%y5#KB??ueD6d*SNC6c0*()> z6s;OuAnYRklgme_H5_RRkC<2|DLyF^XHSnul?>Oegp%{So9$3xeAzTk)D|AKgW?N8 zjMGl7s+T$xk3$+d#{veTXm&n$o`O&`(882fJEeSb9cF3Y4KbBp>qE}$xE05l097wu z2I%;htpnel}30Ib20$@kev*eas;4Q|xXsAQ(h&&=_y0Le9ff_^Y3sfD}5&h7y=lp2v{ z;ogb!EoI38?_Y=gIiw(kYj=7j zi{5(FAb7fGCunpweOyFkf)0Hl@JnzK=S7gRTjmBA!(S9I1YH7B)4d%45vO9|+p&-H zC$qi|PJqMCFwfVAEtdu**nyVSr4_@iy2f^M`Wlq$huN(t`#`X(W6%%6`x->W(69KY zTX3iBX?j;dZ(xEL(nt+bL^4Yk=>smU=pErMg6YfyT`>IVka;_P73abbWTxutRW`S- z4iVX)pnnlq)~uo{Wk529Df@BkBcHs2Mn;A=?ILikvm?~AY#K)qH-2?A0{<9|D~yI8 z4h6A2ZwaE99{~ZZDTFZFqGaf~GlfP))2<~eq(SrUE1c5x_t9k^1$&2TVOaa(#lj64 zIY&Us4|R4kfB$RX7?wg7O+Zg&2IQzR`|HV@^J}appuK8G+muT1O#4=)kXcojcHq@a zK_LId_l;gm(g9(3He;1|L_G-M+yF|}xVUByz<+j?J4YEiztP^TTEVZfzpVhhIR>f1 zZqd8G`b4(>cp|eE&B`T#*0@K4V^ z;a)Cw$gZ+)4l3(Uy>VDxS7yJ`Q9_Kw#mEeD>7v99QhJlNE-;UVeRG0C)wQoLqX?44c4N!{zy4&*44F26QH=ey67FI!R_wHfxK8~ZWIe*=EOBnUt z+`3bT4Uu_8&SQVzLOqSN8{)_g|NBde5__qjVe5^Wa*t|~GyE%S+$CQ-KYS8mM4uVk zP*$is)b{(pebKIkR66$o^Zkm6+M%H-3s<;N>Q?aLaDetT%M#jB(eZB*m`fja&4j+Y zIx8r5?BGFYWHYkcGw8UF^FX(H$YOn}^E+<8>A9mp({m|}O-(xf{x7AZ&fJzI^?XXy z!MjY{ZmLhTiVtgl6-+12%y1qIKWn&lT1(*DY+J%6;Z%|)sh(@goyZu<$BHkCheeW3 z(P*fxyCSwTZX3UB#08^H>G*k=HBO#bm+&iSZt%)w@o-LL4vnQM#f}zB_Rl_Ig+^L^ zt`a#cMo;hYwklT1TUc0g%YOcB-PhQ<9MZC3mhIhQ$=mgI_{<<1Vr{pYp%p=qC!*q> zF1p!&wli^fgwxIE1TP>aR<(Th;lL|PSs`IQp9WP_+2^@Xubig^cjTT?Brlw?cteef zDJTeb+Lzg|GlF`S&*&Rx15^V)l6sdIRQOMTP21Ub#O8y?%Q?dyv$ zR{6dLb)v1UJXKz<^=#d@-w#DFGi*xr+J(-{e{V0?+R^v6^hVNlH*x>I%ug#7u@T!%uHL5ULPBg@j}xur!C`;Clp_c&RnpD zej;*3&eZPoUvG3Bqiuh4+rk^mXnCzWaz_YZ^A#zAT4cS9GfRsWscD{l6L*&T`mzYm zlATk7?Y1(I-KAN_UmCkO6H%9?|46xZAjd*}T}4iAF8TJ?qq$!sZ;jS!?uq(*vdf2TOg0z5>?6UKGL$3f7H}e@L5U}nwY_#r(3qp8k?z_Yww1-hz`;A)|okjvE1UE9Q^J2s*oL@Mb{;KG9J)Y zQ#d*D;1i2}6iZ1< zW4|op-%`eu4-9c$oVQk?=g|X+PqMd_K3aG`9b#AN%NkK&R?T`^t?9XH^7dm7Tlwqa zu8iIt6KX7H*p%)+zu9lb&=zR>OZO>-YIhgWgr%i(a{}2ipT3CPej=aX(i|&f^mDSh zMFhVzkyflCW_t@(!N_*P_TC9Q5&8FT@)k??(gB<&ht+f zHb+Oc#H)yTqBZ%4W~!v?F>$N<)^fh2bJC82T@@%U-s?udiyc=KtpHeaUPzida;Bt z9(8wG&a)@q?__)Vwc9t$f`;!|&hWWsGXnLl`Quo2|CbI*Mb5elcXT^8?P z=vNDRUIwl_B5?f{{&8}C4~{NR@K<KK} z=b0lNHLkhlyMC7QJU`z{5x2OOsX$e*HZ<<%y%v+sj*f==T8FDUJ&sXlK1#yLm?lyJ zR)L55sWWj5<*fn&GB4eX+dFemwO72T1Xr6f<8un1h%V*St97(_t3dTFTec`U3pXyX zgzxrbNl|XHc-nSNGQj?6jESb@vg!4hCqirg7|}5@ik!M&)*}wZneQ^qO4W*-@Dd!7 z7o%Z-xO4bU_L1?YOD4PQsTKTwkMfv}$*!%jYCw$h@q%x*-*{NN zc8$S&M)@Zl^fa#jyKqLxkZ+IY#{$KS-N8)^hE(YIxH9#kWU9&Q&Ac-Hb-ROFS2o54 zFG}yCj+4!O*o}FR*6kOI7kX@Qho#R6CbOXb^dQFQ+7MV2m+ynua~i&wzx%L|nS>#S z;HY}fjTVfIX*_>_zrK?=Co}W?WTu5LS=O<@N>H$ByGgIWgSJdE|gv zeAAYuJ=Y72%*=*nX3BO28#4;(7UH?($hVh{e*2Mp7DIc&kajhUvz_ckS(h#2-{d=H z-=b(9L&f7#+=s z__9Ma*LdZ9>&FSR11X{c)2ZFV&G*K)y}z%2z4frylULUUbv*R7y8k%Zb6gn0R4K&> zNZ8k3E>J76zEVV>mB!ah1uVQkC9h?|9dW&RY@@on`X9=^Ir>{W|ZvbihdbqssY{rbpOrmx#cK5_9UGy#H@>T>Rttzph?X>Ao4X|jsA z>#G!|_?#sLZ19v>t-%s+;autX^l1iSleL;DZ@`fS_{46V>lFEAi?4n$`|e(xE8KSF z^60^w`R`714Hbgw-q%sM<$8ii)#tg&WlA8QfuzMU-c%{ItIOMzSHZ0}hukl@PYZFt z2gEZ5hce`xuu8M%rK#6RyNO4gF-12z1ik!@961sYpy}?e_Z5$g8mAwt4R-WKH|v>u z?8%{(IPxrYRoLVSrEZRH+-KANhkR`816X$N3do#&WZn?}@Em%7($JAGjgLyC9Xw25 zU&r37Hk96|ak8ec9Ad78^8PPhz8IP*n+(KTEY;?!Ap1MW3p%CpZ}4NFF)W>8 zx`e>*w0`}{pQMCBA**VhEUp+Tv9uxo);?H1+F-_%(908^4~}LTMC(2~Vr0Dh-jc$n zAcD3k-N&aaTX39dtpsOrw(E<56NmaPNaVbwZkwgdoR@YfcbKLna=musIZn;UEwgD~ zc!FWT`d3v&oGxDL$GZWmAT6kDFeb2)#?U->v|YxwamhOPQ~7J-E>(+Yp4EQ6lykZ{ zz$r*mx%#sMcK+IN5|KSuk7mls?AvvLGHw9RfDkN?O5kqfD}kYMM0rD`3KFh8Mq zWyu>NajKG?hD8bGeEHnP7c!56!}SSjlUy3%lAN(nJ#jD%0-B=hd_<-fdWTufbFF88 zgp+52z?jw?J}X|nbX(%d93B$4QEG8u=i#j z1a7e_5|wb6>gGga&WPEFh-huhJi`+eWIi{K^^&IYx7Yd7LBPf7?l`jlxo>m~m) zpDe0A8!^{*WGk|MItqoQLpWtQTS!#%?CK71DZV~e;z1rUZd_kr?FKR%Vma5^8UN+U ztX)J;vJYC+YN`Cg=cEG*ckfvxy*cvst0MbiDcxddDC{WCzC0rR^yR=?$46#HY{zIx zJSc*z-)24~EtK_+^s+LL>Wx$eI7Di41UFI;gQ~>KjxPvvUNI0^rqa~eH!VcR;S08# z8xKd9yN_qr+C~_$ay49}jl6E2b5&H%J6n|=HGbGm&S6@Ki-l)3Ogn*XiZ3@s%o!3a zL*~0Umk0>~Cu(?ktp-YxJ9N3Mq@>0!1yG2b1_{eNoT=n<7QwDX;_;biwL#Xe)&tSjW4jY@h)ez!3N!I^e#Tlfolu6lR)&OuTo&hU_|)L$@a^-;@{3lF5?eD2LI+1;-fq3& zAC+Wvpqx5*pT67OLyXgG;I`^St5Oj&;!}z)S80Bv_q-vvB)IidxF*%wZZDmc;jLMM zU7zC!S>&V#-Wk17ebesjBhgm5E~qnjdMTNvZ8){lto&*|#Jk^Jdn{2{r#P_FZzrYf zR=u?52%LY0;i3}&kbtpCsWn*a(|DAk?0F(i^&?Hf1urb5MGLK){@JpFn7<^!`M`#* z9+GHRaEsQ2N@@`C&|1Mt(my=59Pxr}vqy}xxEos6(h1uJx2^eC+^WA{Z}8?RPucOFj6cJNGpk9@Ge_~$!hEh9HJeoZ7E7zhDf5{d9_Lg=6QC%LMk~WfUuW=&;-!^#HeHizm15k>%bPG5>H{Ux5;M~U zfdnh>*PGpFhP4p(3B5FW+ck8p$0RM2*MA62+n?7H3t>f%;>we{nh||S?gSte_i)RJ@0Ff9kt?dz{jt8p zB8WaYc(xnaY8r=ARYZCpUfHGfNR_-cN~Qay^_2%;n008k|6p=#smB0s9rGZ_r1(}s zrD^ev({Wwpg}dtfTlz}3t^hC!w}m1_nxhMp%pzLFjgaJ?m7zqTxni7XCq%p$6=#Pf zO!t}fu*g8mVr3i?drMFT&*S3y63c%WREa_0ZXP}gXDiBF>z9c=b3*5Jvnjv0iQ^OC zA?skSP(`kz(*$*HNZ3y!_x9x~j({+VYCBl}<4|fINR)~B02g$RWKmzNL=Mq*gWY^r zC1tWXce8uZsaXz$VuPzqmIrs}T&wH#YPeEeLvpDdlBEg=Kx{ATD8H9pJkImNsjp(h z_*H$A#8c4wAjsm*o%#{4UrU(?g6dWK2Dvdk(+Y9QPe-3Te3p=Vx&{N|@(%5@i%4s0~^8`QC^ZX^)~~QT_c^ z75&kZfOaVsg?D-fEX_|m$h}x6*YWMX{u?Lkq`ce0eIXAm`~1d3F%2g@NymRsO6;rb z<_zL+nfL6sm$=gsYwwphEM5@Ya8yoC$JpcK89V{ORE9`+mlRUxGkJ6|%rEQX1aBRE z{l`>(k+Uu?+CsUmc&my(>e*OQJT_2k1uw@?ayK*KlB1jf7l>4;#&7Ty0v8B5VC#O@ zV7_9nK#j0Os*kZs>3tXnMHIEso;yhy9Ss1B*2w`OB4M|b8L6wtjea*6Bh5YTW-#U) z9Mt$77(GCtd|WoPthi~Ng6WB;*JlQ+AKk!G8>rN19;v==n1PoX!Q1|MZp#c+F>jD9CC+TSW4T z5XE5!9f2M6$JG-bN8KRChirNRhf>iNS>g{hBn~}_-DHa_c(wP12P6chYm0+q0LCOm zZ@HS(m1@q1r0pqN(%SZ;<4eWjiYA_dy{vNv;&NO-AkUk8GK{p@b0_!w_)d1O#jy|s z05Z<$Iao^B>n1LyNGBJ&jds?O=!mz?QHCHES?7nZO2}ke{^*cvAThf@Qs6Q?!D5EF z6t@IfB3^2!v<8D~$}hA#P1^5do9}F4~|fEdC*EZ*XmV@Z_R`@H^X_I(dF2^6fZxHo%%9avjp_dGk_NL5104d zd8Om%Xc0GLfo-VNeFXg<%BLex*7HL1Q~k?{gJEK~Xb1Ck6sLxKf1b_{>R^>>k#YNB znl#^Xlz2gUqb!mT|DY>9A)cn)R&{7qdI$l42XE0Dyxg({6C;}s;rl9oha%+JEgNL26=@uu)bG`LB6nEptb(zWUm*Ez?V%8e}mgW8KH~kfk0Y$&eruOtfLo0UOgm6lK(9 z_p$1B9e%oD3`|jK+0wXeoI?5+41~84yY4j~lsPPvp^5X{Jeip3ae(tWUs!-1u2u@X zcT=^oO4WLg)L&6O%eFWNE9txgB3G)@5O8e{Z08(c(D&VhwG@8fr3_GClT)3s3Uj2b z-QkqnO-#6gnRmO+Nc&r?thnR^H1kK1jq2>Ds>FPdh$g;lvd_~Lh04sdpWi&k98~1g zw-#$9xs6;9W)3lE!lNudfA$IOzYcfc6VU0MmEIV+L)3sCqG zAdh;dM^C?>6G0jlXTI;cZntp7q2W#2NJdWFd3U0*o1+^k@#V57^Ta9IZ5Ad+oLBco zG0v*&-{~XMc*zzbl@IkC43wVU*S8FOr4~8k5HhROT8# zS(3tL{@W{=g58J}Q|@D+xNNlc$MUiQ;Q=`6L3N7V1xg;*{vG?is?>)Yl`LPc3LqV@!H$%)P3QD$cWyX@ZtW&Q93rm|% zk@wPqS)0e&5U|;p#rO{&xb|V>T?WsE5e9w^7&%5;V0~joWfNdpAl-dmPOiLbr6|c= zHTtpT~b8-u#H1ESgc3Ih3 z53;+NcCU4x7gVOf$_bXO zZy>};TAe%6M)dqySjk9dX-ASHFcSn6;CR6bQGRM^}o{ERMx+-Hi_;!(QPKHWV zxUi^3TL^v#_k7&eGg!of>I=O>?zeGjIFXG#~{#`G{KyS|cyOMU1y?7Fo4t62!fgCWDbMyESs!6U*Vos#u^-2M?zF28FnKNwj@ zD$L98#I?7-pO5pLT&Q5N`sXfAN1wSMxcxy&W@fGuV(W1A^?NcZa+vN08vC{=DWwd2 z|77okpPU^HdQD!072MbFI@+pi8MB3QpMf_|pXhNt@{N2o&A;WrCX>FRj!ZLg{6VWT z(qG=6rF3Lqrn{^F4JDnG#uc~GyRKiwl;>Wq`aJOLV4N(;oyH5fLr6WP?L`9U6fevA zzRAjp1Oe_t(hPVVg&=i?bK#W5_0dz9j36~PwDk9zXQTt&S2l19FV>WcMUY?h>W0*# zACBhY_Z?E3p3yAVjuhH z1is=m;;|CJeY~u|GTjY8rbbrK@ym0P6MmG8bzY<+%tC3oC%nTOvuWr)Hn-(^+5F;6 z3^Q68G$oQ2RE^ig7CkQ9ZB|VlvF1w@m``^0H|Yy! zWQU} zcuHv@)^+6NyCS~z8`X35km(V^5jn&`QE^|a`+n=5yZRFpQcRkXSZ+oIeY*Y6Rdn@L zUj@C=v9vJ`gCoyNUMOfHm1CR~f4;v6ZU}sE$cF9VP*=(}OY3=KJqru}Bdl}m@|~sy z3{bJ$4N&3KzAzst+WfeBU^&P}oFkY*lAkQfi9g|9MV`MO=my_3hC5W&H`ZJr`dft-pIu{S8@4g+DpUsvzu(rZ=tVgQNFO zj%Et29NdemH#tr&aT8lr;!nl`(HKKnyI-at4=6PE4_9h}hib6L6ZE+hsSWvSA#ZS6 zq6Kp`d>Xi!v@P#OmEFLHLZ=5Y7E_w_7HPOG#V&It zOYTF@FP1D}02VlpU~5Pwi6Fm#0P`aWjuAE~lAxau6^Ts@hlsr)mzXyABbPAvGd$Ae z$IlJNZ~lBgfk&T1SSBQ4vhZfB2hoTv0G-n_l=8f}5k-U}?kTWMbLcHQE$h%L36i47 zqxP83V@Km`JH|pli1mkAu1onr^IYR-Nh`sim}Mb&!bSv3knd9vTGaP0HrO_m*~-{A zZI3-bs4nf&UEKsF0}+D+@l>PX&mUXOWG=Ppa>)&_?&ngr09jaM^7s9KN1WTZ zQ4~>a*?vr)`p$V=|Dk-KFi6Y>hj(c;y*tJA{P2amyiGzNBSF9?0P|BBl?qyX(z*bF zX#+lrXhUAj)kU=T$411ALDj6yT7 zVb5AhMmeZoaCgtj-2!W5%^fl97rP8CSJs& zzoi54qNx&zpv_T9olcK`4B`L?ucpe$#NBQ|!roRo_I#~p)&X&4PBLnKviv$N{0D=F z4NOUB0Kt0cG6RgCS)R4(w)^vs`{LtojosT`THhquxtzcKzVLVaYX5C)Ql|R2xw+T+ zqK(?Rv}O#F6tK!)H#{&*c@5G;#(TO&8J*f5Y_Unb)RXVfGQGE0JkPd4s)j)Z8)Dit z#*zZ>?;&9zw1xhyajJG_Y!FfMb@*9IfBW_Z*KwB{++dKdEG>BDLF8BLGW7b8SJ$3g zr7CFF91)ZNw-&28N&~@&f0|0b%y)Kn#!2qbeKyBnV`WJ()qsnt@hcH8`~6Qu{Nuh$ z)ioJgwI^h%<&OBs^b>k}CvI_}MFxWcuhuul zieG6?OmR}<4VS{$#wyZkUsNPPhalMrseA58?r$KV2{J`f?{B$=41vvdp*som^F4PI z0SZzjB{fwIj3YaRUVme~|2u<^_^nnG2Vm%H79^igl9U;DgwLO@7Hy;4+ptlcfBgp~ z_8*5Q_H!Ca3#@Om(>u7h7}<`jx+G;STC?W1=*=r_6CuitfY8)cLH&QLK<_WOF@sUi5C z!KYNK0{9gkQF6JiXE(=enB$#~isA!jH?e^MKH*+PUV2o*CUs@{tZSTOo-bVzW1DFz zy+_$MvAx{~<#9?+FG2U2wA2rZ0R8K@%6v1D{0%o*@KEO7$@ue6aAb{WK=zubR>BCQ zEJ%J`?wRA#$+0Zo;!Rslw%&<{QM4)uwJI2w+8Mn5L(P<!RIu^_JFYEbZ|oK+8v z-viCg1C>xK#BJM8q-p~#J34FOJldKS<};778toLcC=s2D;jLRMwBP-SP}-Jg#9VcX z>fD*LyR`aq4KO-!Vm`3tZY`)ti&e-vGIsX10f+m@K}$y0AQ^0KqG^QYF9jj zW6yp>NSdWg4o||6qu_CMb%;ahO1y_)m57!L@i%OH6(k?AB|WLu(64W?!PX&e3A8jW z(_M$BK?3A5Ht!06sn>B=ivQBB$z;TM7p%ULX&FPFIJWnV0pM3{L*LkhByXl^<3 zXT(VY8|5-Txwe?`?WF?HUMs#-(f5EsqC#XELow!Bv*l-Sxd?4$7rY>!X^-e{?MOwu*^~y;5 zd--ePE^;6C2;3Q^yT#7M5i}9Z2H9B_d<%oP<$JYjq}VLqPskPz-(k6WtQx-<{yTJ6 zs4#zL9b!(y(mj-Zb*pXiH0iUsT@D8^W`P@P8Y{d>-7n!H_7+}2{{ErxE7v#>3wl7@ zK9jnDsV~J>1Io_~g9zJV}0l(od%u+`bs=jLCbOz_#l zPwM;I9&ue+=cK6+2RmVZmZBNAzBWq)4lff8Xo4F0QF(b?H7*Fp%dnK~% zCC0$nE-@f>wNwVQZDn4Jc?mqnr%TOgGe|YsFGca32lx;sva{9GTRu#O<3EOpj z#G60*zWvg7vg%6yieyI$72Y$idv9AAwO!l4KXykaW$no-u_CxqetI`T^gdF*@9%)8{C(+|M*V}PgLN`7CA8V#j*3v zn9@x}bQMGCKu6qSe`whQ%AJ!|e_}E3HVAino4%&(d5eVQgY5_ZGLl=;46NMn>iv`F z$zvZLWH^zh=5#qWs`r07RrLc;AA!aDp(F&R{eajrNoRsFX9ME&lhwvQfbunECEc1> z5bl1w`xh=`?&t$R+u1i+^a~&1M4qWTb7!SVtblAd07l?3C{fV0iTY=PxkgXyrP9O% zjdv~LE;|anAO2-+KDvjR>lXA?Pfdm-QB7A#sNStA3RXLk1EepU0H}iyjdws)POI8hKa0c*M#&zR`^uUZQVu{Y|zJYv_sTi^-D^nmu zN}*3r&s)aKbUGDmR1Y_0yfNa?fq;TYha3gjZRJG+Vads{O7lr$W4IUiMCnVG-IQk6 zk!2Towx)#AWZ%si5lBk>7)y+~sRyViB$!#t$`^>G&a{#1ZPA}UJg~Q&pA>XqEE8qN ze*%Wm#%Hct!tgwX+YPmYO51PJDgtkOO-&%L+KgE>@1@JOJA?hU#R@;YxhwFZt!>D8 zzGvNf#+{~fD3N3Jdyufuu?xl@fL>G~JO~7M`YpEyJW|F7YFox=Gy5;LD&nqD%}u!P8|Y&Pux+6KM!j zc5=7J!qWq4LK9M9l>JvmIc-WuNc6%A@IaBcE~r>BgW>DXA4%^zWSCB(Q7<|t{fliN zh&}z&vh|>I_Z#cMo4V21<&pskSJl!MlFACM0|p^vb?+TXyu?{I)rV;T zauX!~NI+HWvsjX8g%$s1jxk&TcmC^$C3oK69&*Ys1K2Oyg`bbd?(Hb$RPsqKAV16e z6M<7lXbf0sbs(8dIne@$CZc46jj~bwr^s*I!VVB(L-C!(2xQI|->n9CIu$Cn1Q9R8 ziI6!6=naAfHTPt-Rm#Q3Sz~ALH@#mb>fM{{zQgUTq8=@X4IpfKCquH?xk6K|fc;D_ zfTO~(vj|r1g`b+de4TbyF=M$SrKWW9I@5&zPIr(3fIbQRMWKwX-I<~D?*xi`vh})W z0KRlz1Rdnu$-wirBTv)+nZn880a&RKejFqVyUrlj3jZtJ^Pg*_%<_;d^nh_>f(23( zB=K{eIn&@5$;GMi^Ebog0rZeG)E)&eTa;c)->e^fu!kQ6j<*XvNNk07jW7sf!%-=@ zs_gaP{SUMB=T}`;RYz;UFgCnbU|E+i|gmvO~9MHsIt!B<_CA*eZ1i+@SPH57)S6IG(y6+*L?UBOvcHz`YVO-wB z-EW+|E3*s2to{pWXAdig$rWrC=TlJ2K4jy`P`dp%`en~2B&bJ~uJuSoZ3fBe--xx} zll?J0G@(sM{#rbhNu)K;RU2@I=Tyoc6S}lxpdtF_-d+MdzSiH=DN=K!&D{wrqLja} zZNHb_HX@8-{gcdY@7FC*PB8l9+1m=#UDFpRg@0|=U1Tp-R)&w=i@nJuee>1IUbMJt z`Vyu4ub+9$aR0F%w0fFInc@o()5XP9M92r?Rf9RwhL77xHr)Rj-fVSP z6BQ$8SV2Cnf}%0!YSI?|bsSD7lv1J)JvFP`a{XQzb?JFUK6sY7h?eTfb0^SU8mLm{ zTx}XBh-CkH9E}*`UyUNp%Gh?l+c0)-ud|D5txoe`Rj)r=fHcX5=UJ5o zRn;;O+oOG=CWtR~=HutJ%oVtDo!E$Wgdnf=~>M^nRhZu8XHfI#wuu{ z~S8F~xZf7P-PwToIWkJo*pu{Cf>EElB)dl%-5&Eh{p;Z~OP1 zs?r^NAYKfqD$QZ_t6VKm`p132@336#tAqTY6q(ljPos3{(A%5IEiEol?AB6VUOPGr zJfI-el9#xYY~9&E$(QLsj;>gB3s=NAHY z93jRnS1M<3uT;)P-$5)WQZUO-jQIw&jEG&|w%x#l;}+7T4GFD65b4ohLG`fX!q2m# z-TTk~+1`g!IzNB6_jyi6(EIxJ?>HtITlsmn0QCKrLS*6Ft0szR-IuP_Zt1{5lN|;= zO#HYC@*x#P#+vm!$Ozp3%N+xif3+d@bWM8F5|c{76}J87noF5JJVI(&D;I>1{J!g_ zq}4BvA(zYp>5za0!;a^f{+RW|5qE)3qn z@8N!8oh^A|OSa6K3Bi4b9z4=m8?YZbt$8Z#+}zSe{%NESyMnDg1g&~=ChP+jcSHM| z$oz28HPVxWm{roLTRC4Q8ZfK!!M%Yh6JGi?F+a`4OgIYvFyUgRv}Jjt>-L^a_x%9H z@O$FT#K3KtG^a4lN{2FA%LQ8rJo=z6F&_)Ck zUOW8Uku&_kXqf={tKEAb?$uZb!_Sbk=ABg|e=9Po=`7iwHi}bFhU-Pl@{x*qU^R%$@ z@!#GDjtg;!O#k-&?Ok<$yZ-vG>u=k7xVJ!jhuR$l|E=w(5C3|5*VfB{b!>c!ELjMZM zzP*M7!jG^cn@#Rn^B=1%Yy7>=WKLXMeaDn*#smpRm`xj|J{B6FHC~HgTrk1 zfQZLvYK7CPc?xib1jZ}A^oE1OIV-tJZLrGg&D7LVYxU)zsDWDY%UV~Y&eqQ!^E_{8 z_@=_iFA7ZPwmg97=etMQ>fO?FxL0rX9sff2QEhPs{Kn+>&3XXnSM(?e4JWpDZCSl+ z>I+D6;WBf~YhiFW_a~ZzOcXpyT!$r$tl;rs$m=5p_zh?p6@;O4?rSseBNmg? z@BP&#&3oaq7QD4`nTiio$jXJent-JJ&o1;pc@KPRFYE4=YnX6Qa65C%^Qb#j;m=#m zL0M||XkftRM(6<~E6D468xS{^BU!-cyshwE?da~P^Lz>(Wvpzx(5cnSKBk@4fFqsr z-)3hULPu{PqZYJ@L(+S~hkK7w9Z1#BTPgp#GzQL$0iE~kv1@Z6-G}2uOt--HzI4c@ zQy?d1p7(^$DHMJ6(rs3ttnL{1=_uga4K1=hiVTbzoF;>m3)qWZ1m!IC#Hz#KSJFM0 zFyx;2wyX^KxH!NS{0_!h{XYr?vw>2EBL&*KetF{DECZ~=n;gau_rx2Y>gUGB*P8-p z!gQ$(&N@j-xaH;bAeh(aZAId08v^v8{xCH!uYM%#`Hs@rvsMO`fk^K%yfzu%COt7H zf*K*>=?P7g#p=q~#tmG3-OFlG-H}wN=uNK3_B)Tn%pRg>MH}34pkO0_SJIk)wLCWd z%eQ)ug)6AQgVm&45inN4}1r|c~t zBY;C5YRUR>{OkSy^GASP&Z&PuQqGSLdKmBwsRs8XTp}G}KDlk(pMTiXa)As(f>kAh zKf0Nb`^mx7fvCTrmi0fEo!q7xe;;JRpk02`9K`H#3Sx(IuP9AVp~6Yi>ytNWYR#Xw zgnzK7K*{SqZdu2X#cq{t$k&&^C+o;*&2^8qcHnIO+RXJIh7HfporF^<^tIC(4U^{A zOaK+abiSwfK^~l8P<(szZ7+!X;Yf;ouE&6xy>Lzq;c30Zx*Fm$m+{jy7(9^QYkD d_y6rCk}ld2NgaEwb|KBsQqxz>K4tCye*md^NVEU| literal 0 HcmV?d00001 diff --git a/.old/tracking/notebooks/img/mpc_t.png b/.old/tracking/notebooks/img/mpc_t.png new file mode 100644 index 0000000000000000000000000000000000000000..8ed78b4b311a6140b20840b54ca041494868a197 GIT binary patch literal 32576 zcmeFZhhG!h7B-9xv4I{%5d{lk0qKM)AT5-HP67l0LkT3IrjpQ9P!t6e6e)rt(yLVI zDhf*Py&P0(C@NJV@a~aw?!E8(FMOXrgkdtXXYJM3v(|9W*ih%dUjDrt92^JqbhS)5 zICj40;Mn$xixZsb_;T+m2gly+6m3h2n>P;c>ck-|qq%h?EG_BcL8b`HXbDS8)9G|^ zM-rAUj`sk+fsfog9Pv23ljGLM(vnh&Vp39K(u(F%vcfXzQgYBY8F5)jY0TF3SZ62q zf7UYaVB%d}vBJ`Dn7AZZ>Y}BTl(38jI6~P|-6_J83$E6CZ&fi7jL`< z(#*~jX#^I9HbZkFk?|hxTbD^mi_3`1Y<)mscsgyJb#$WP9h|n#dVYXNtx+ds=1gQdg(>Chtb7;lOz z%tID#-~e_&FazK0P3U-Mng-ob#z__?ttjP*k(84cme!E=G}iD!Fq{?ab&Rw~ii#RE zqLG8Pt^q1q(v4Gd%uvL;9!nw&8~N*C!$bYtpjI6zAnAvCqg zfJ&sf6~SH|=b~k0WFbRvf*EKz!H}|^mLz?qIgKC4B%EOW(fvgK&M*#)ywW7dCG6ZP^OiLdrqlvb3(o}b%$!k(g9XzD<2u`LX zPh*O-ff3dof!FeY88IA8af)unp1SG^n&56ND{UPu3nOO>1)QP*(bU~d+ZCLJmUT4H z2JCTn*VC7E_XPWeYiM|TsMGYARwO-B6i(lfXk=hz=B{HVkJL7`!Xvc}NOXH|IWJEW zFE2R+!NtXmB8k#4us~5A9pFwReM=I|%}d8h*U&;+18t8+G0?hND1@XD7LP@uoK4XV z6cf6Xt}OUm%Gt|B&-Kse>JE-l#-^r5a4jiw4V0sy2@@^vrRj!*n@Z7O;2~bRIxwuB z1KNS+E@|p%#K6IH+)d1^6fF$Q;cyEbguRym1LFY))XN*oVsvGhvN$&vOSB$>&ZJ12 zYsi`65EupUsV3EmXas&5y1FyXWlg&Y^e#?cA&ahNx8}z zI=H#Hn94g5@g78DI#|aGkEKXr&}d0{DLF?eQx|hbT?!VDchqo2gHOR34IGw&a*{%r z;faoDM|YYl#t?^so0?L+G+lA-F5n`jqB#&2$sBDC!&yLjFGFD(YGGY5WKV*DyNR@- zE(&LEsHtg)CtKLdTOjNmtxR+^Er5{WE=Go~9u5vP3zU%s9i>Y$0Ox2{Omm8yj8D@c^!xRW;Qw3QQX_CC9CQ3?5lT61+%i_SrIFgsCftQmf*@J;*m>Q8Syo}5l zR6~M}y*f%-&)iaqq9KJhmsPjNA_-O|?lStGmUKLYhA@%?bQ1OG+6Y;uk)bvQPj--z zw0DOiA$4|?p_!Q)o5~q#($K(>YIy6B>}fidG)bbRt|LlO18eU8Y6s=|HY(zkrU_Io#6ci|? zZeDnVffrd3EoUJmC#j%_hS?ioEQsD{XID7zof?`rLyU`_qZCRC?Syml^hTQ+dwR*C zffv=#baRr{a#eu2V$I=bM~b^SMHlV@Gr-ClqP4-tt|U)1Qx<{sM0hF6Q7~>^UT#cJ zX#$ZhkMJ;Yv@k`Y734Hz&0W!&hJb}KXg7+2tge)s3qb=*QqXfHNEy=LQf7E_c|}tu z8Y}7HO@-O(dYQUek==B(&2jo(1_%^F9bw=^K*)GlYC0%*Xvmwm(U4YVGC(F8GIV!O zV-kgdRWNcf*Tj0eT53@hU}lz%@^lK>lI}*sqUrK9BS%*nRzVNxZ0h19r$BNsHghnv zM=8phqsV$BMOP~)B;@(Xj_y`qNn?bu0?yphSphN>b?~32tAJoYZUTwcF@(9wcsL>T z^*ulj16$E>mvmJiQ4L5qNh?iL4HHK!LehaD>*Zo5jb%Ut zlfA$<7ZgL@f+p(?C#svG)zz^sST6-vh(CB2FHbKgikzp0s{&KfT_4B>ZcLD|gfk@F z@s3Ps2FXCq5GMmCnqlO0U1&^WI@a7+PR7at=BO=&l-Grupj>d~X6EjuC^EwfVM;{6 z&?Z{u21r+ZkVU{5AW~^);g|+S79a+~$>#R*+6Z+AXR?m5i!LAxZf1^jly~${H&EB2 zBaKKJR8Op-tGb6X7HKL?b%W~}>YAzRxs%XLCfr!VO2LIhk~TGj+>Noiwi_KKt)uIy z=xIiB_EgZ4b0U+ebPcA1tceNDSPMpQRm3W4YfEArwDpL(CXOH{(y-7{Hz0d@qD&pp zQc^~`dKNGwTwTF}3S+7}p8jVl&2nDlVNF!)pj&>FoR1QA~ezZ#@ZMt zd9l#(^dNa?z~KmOV@Z2$ZBtXSlbNI*0^#j!=&kFai>5QF;4-9?6-LSm@9e5UBkOp> zH7IiC4wAs5Xc+37m;%3Op&^OK<0+a3axhb(C7p>gm%$nsxtJMy+0$gS6oCH%Sse5Q zq6qxoOyy770)GD|-ILJ{v`ri5;5f~pr=@Q0{cL7vh2A-~F~7bLz6UujQ~4Cx@@hqc zSHtAUq0Z2d-rkFr#_DCKFP_zXdUpG*#T#6&-B^kv)aJ_O%DH*g1~%f%nZwaB`(qwo zAgXlRiblq;7D{uxE-;$dOuNk*$DKPt`U(CaDVs@y$F`0+9&p~0+&V<=-HF{giax>x z@>S@c)W*Z;pD;KcvK z!yS#$mAW!G2@y8+51VeZ3vun>RG@FXY)&XQUS@6aE?MUw=VFYM&!e?X6W%{Oe)~Za zoKmE3*voMxiTq*qjeEhT_e*kG1pem|t%IC=ftl~p_Wlvjb^EhiHLsp#@;}JflI(fG zTOdA{X8h-ZFK567%P$C$8CznGw&Z#*nXZ!d-#d!`-fY6rvP2p zQ276zVne4Y*&Fb9OHJsS7D)B!jjFom`Z>VpX z(OXAgPkjH_9+LC_J0E;=NOUe$y??Tu{)1SSP?2=vb9vdp>O><^p2zF`lCLX91MF1N zZD+%MZI+4eK4|>Qq_;H{GXV(j${mW@zUzNH@Aa7zd_mf-2AYB%q{g=N-d$kRMan->dUXkN*It3U zxwVa+&;=W}Sv}B4@x9B>t@Ea+dS%OQYd&We9Ijbj3qL6LxhK)`ztqlS)uTUixumc@ z&(QROg;gh|3KULN9j1*kag>nsB8AApO}3^En}70a|brrnhAE3 z-sgo*2Mg!0J}Z|+Pd@77`s~V$FQq>Tt81Pn{G`z;j!q0lJfKT2$1_bU_hN`o3XAX# zE>EjM?b@Gwa1#iR@7g;;hzT`roVM>wFf49-YYD^6vS-hh`T| zm(=B@ArGSr3lGk29!A2)w9X4p*qxEnT~EG=`C%AWt=~VNa#=*m-EA!D@;tX-y7#tu z5N`oDY+2k!A9pJ{sZD0>VxeYSO*vK0OW4}PMdqw#Ey>bebJ%QszI694?8j#3*Q}EE zYINND=hNlhIKdTo7_a!`i{OH z2za(8S?JKq$fOSrOuLq7=`3y5)o%a4*jL)YJw@qh~_v{o;71qxcv`Jn*Y z;zR5n!gO@8(%WOFFxOi5_zNBP@LAml6nx3nB{S#3yh|y)D-UHQBrxx}80Y$SCBmcW zg&1zVr99%dI!#D!l9BnF1##ltq~0#honhyNUR)TINCR94+Ns|+fGFb@Lg^kvH!8XA z2}l8^B6o-W*|BPc8Y~=jfD+|xThGEQFj{z(pMT7;t6qPUyRV=B7tJ#XY)k2&%9&JQ zsma&L2^RUAP2)nW0*_*FzCksEyX|u6jg5-K-QOpa_*IgCoko3ALL@&toz+T}Jie_+ zv1}=b&G<;FP_tMYK?5~8p`pCM%nq)c2#Ig8@9}t{aZ55)I8pSoHP2v9Z?(Y>tNDzG zEX{zTfra6o;43)b1f|*_FZ9=P_y4<*x2IUxD zkoO%DAWj{wmYpG1Z#m@3!-J0(erVjvdla^UyY5HJv{oG`hD+N994ziFI6JWbEEfex zv}xAv%Pmr#a4t^$n0utqKiM+)k;pkNQHsEvvt00z!fp|JqUne3S5JM3ka)xD{TM2n z*!fcXiGtpx6KP=YHKnJ^g-LJb8BLoI5&b8JZ6c_X)5?QiSiPm{PZ;OF+J3Z7=jr4# z`X9NxJ$?CU)2K@9DAWGa!dl#Lz_IF-mB0bg+7*%(P{-NyE79bcE#mKNt|PDiH5}Jd z@xZZSqcud42z6mwbEf4!w&lSu--oot?ptr($im}^kJ$O5XsTq){L)dDr#RnY?=ip1dO^Av`vs@G1gL3UZ);7NfD8G@)WgP$P zy{>MNO2#(#H1h_jO!$Z$&e7VtFY^zH$b(Vy5Q#wj(}8gy?pZS>3Qdmeyz^Ocvts!dP>Pi+hj~cRv-u$>GB;uu87rwnVyK{Wmn$# zzTbcQdmKLgKVd3I$FhDnhxqvvFcSHRd(`y-LoS`Eh@aOnOpHp z?muzZaFCxOVt)o~6s~0ZZd)T)t>}3!BAJqTp^Gk^s<^_!Jkak8jlK+b((A##5_CFP z+*qIwNc()ZRB&ER@wK_S0ez zD3O1~xU3xp3A=PIc%=`x`#v)fa+ovsq@|r&?gf=i{vS&FkCebz@Siz`AH`h~eA53| z5r6-~fc!IDJNEyn;2!w#?0t`fUoE(11T6)eYQ}5&0p^Tw!1#x%T!#tX;%**hR_$`5 zm#es4=Y4<;DiW%YFMr}nTwRz+XRfERvw!BEpv`E2)Z>9g)f$2X*v zhnwry`;Tmx*S#Epf_vuYf!KLlbUWkHZJpymC_Z(82MLYKv&_#W@Z7bLww~AIwZfk_ zz7!vY_8t1oW?lLFyN4b7@Be348?B{anU2@@`>h$4yI)?Q$a!f`e6a}jt!}#OXq0#5 zMD@$iL>s2s8&vSi)cgTi@)AmPxjOh}*|YokJf(zj~-*zml43C5UA2aHVGprxa+3>b<(PqRTv+_O+sNTr$UF z{+`&gvH&UA!aZVH)NhlK57j-vi2@n<)W^QI`lBYCSP)|$W*b7Og2tCJ@eA%#x-$>Z zt7vB*x9soP-M>D-Z1jfP>#@ZGN7oXz9E$(KP(z^_^V+e)@@usfFDoA<)o{ifO)_n` zlDX*g?M;=@U?)(CH6@xMamR^Z^MWT|s)H|+uiDi$LvnSl%oLnr6DvSIwxyIDe2DR~ zr%;e@5z?vm7yBAB^j8;DKRm98ArFi@LZ$FlDJ2<|HOI_bV z^BHy9KP$ps3YEWesQXT<;q1MWje>mNgV?w8?RqT#Eo~kY&)qQV!K6CXE^3~BaN0H&(dMwQydF*T>t9t(-4vy=;0sH`Q^y)^MBY%v&MxOtvbJ%GUEWZ>H5Ni7) zyNkH7QmB+?>#RX)awF%2CZ3fCVo(H2Dvu52Y#>J)d?T;vUK# z$QoWdFr6J>rv}UFXbB}QlE1RnO1+Z(-PfM_<85>7;-0rjh}~ZzHu$dhuf6dn#-wi+ z(k9ii#nvjb^{Pu?(<{Zy^;DRV z*7NW0h>oA2l&=}o56C^$J~BybIy98Nu0dzfrrp=iubXe)^e0%xrAHB%Ce^ZH9TH7O z(v>)lFbn|Qabc{gfIeQKDo9jwF8;77TjDE5X~eNXIQ{dz*p2h+LdJ{&jN06+&^ z&Rdtf6H=g12Sr?)YflH}3&Touow#J^8CiUL1u*6fa0Ao&Uub=sr(@GX_Pm1&?Jtih zOFz{XKf9#gKr^TR$iPd9w>f=mR|txL9^_lNgq^)zgh6uvLPZ2qiDpQy;i$2dQU zV%oO-)q=o)^cR#^E;oHjfgl#&u#Pu*iM1hn><#GRrm5t_{`iHiFVU&Ex~~M!M$>q| zJA@e<{Q>32g+-U#iD;0~=M~wi6SIGJ^D{1^NLCRf%P;xIAHD3x7aFQito%Y0D+m-H z*^Y^JvopcA4|!C)|@$3ZUp zL4VO%`<|b9HV6%Fl|vMQ|J!s$|EEfR#|3Ol`qv7*2MNn>y1vme6OAexl9Zn66p4pt z*a#-a3!+LZ^c7X~6)HoNRRGD*SAS&&#hbU7eI(z9H}BVxJjPhTuWv=nvDd!}MQzI5 zUdT4==HU2v_K(ZGevo`s6@ft&b5j*2hyVdgSgFd9-@yWr>M&H$2Pch8`hLOmX=bu* z3e@rCUTQd3#?N ziL6X{&$#y0WNln!Y5lGMxCepkQZmf#-ig?NF}ut4?_X&l7PF?1cgLc0(z5kziDfej zicwFbZ}4TRz2Eiy~@yc2J_3-if-A1&4tnej`^5 z?BvZ|0^+TdGiA1Q_XnXe?P)3&Q>~F_g2z{F2n%JX+4>-Kvq@Uo%ZPLeoOmCA!EZgc zg5cMubrI&kWXgtGrnN_^i&P5@0G!2g{rxj7$hs$zPLcE123a7a1l=ILfNlC<=4_JP zSEP%SCNANLk#!N5+1EmyaW%PVHA&pL)<(BaGR0#1V!dB{Z#TQw6#G_!lLL^@}74^}sCT2Pn$rbFh{8X*fJQB@anKcR*vqC4MC z4a92z$U`WHgY{cX()qfRVn46sTKE!+&J>!llj^8vyw)G>g7Q4d<+W@m8aQ}A?cS-{ zSzX~lcr?MFDjmz{?@qr_1m}h4%`C_q^3Jw}y-}IwR#>wCkVF3q4eXpnNzyB+y*{5^ z@)a0pZ+;>=av(dpMzlv^Pf>`vuo<5B@SQ`qwsG8mno%Hqd2m1+VJg;K%B(FnW!;J^ zlqZ=VJFF6ZZW0vXnEtm7Deil`?(oaC zKyRS^&iJltPSsWZhk`BKY)g*rj%SJyI#NXLb9cs2Uw?dMS2vn|Z!DpOlo*j-Shl0N zLS#0l#85~JwA6&R0sx=QQK6Uobrgszz?_SKpaASC!svzaZV(j-I%BGk#ekGn7UVX6 zEu*a-)aSR<=u&n+D~qZ-)!GA};&BP?O64rEzJJTGQm_C|?0@NSfVXT|sjTaoN_Ge3 z<-1ocgr>ENPHily@`M9@)MTe`18ZyXMCm7f>kaqxQ`%YF3Pmbzlh-2gt!!7H{?i;g z&gaGX@wPMxuI#AxK=i*1NHJ;~T?@&#Td<5h#_@403%tV53pe(%zCZpZ$|ZT;&5e7u z`$unIgj`1Tvp310PVeVmLjw4b1)&(8%@!Z}FBTjUVV}IR_v6k(EVHOX;jHO>2eq`B@suH$mX!(*-4={@En4;J9FHutEM}waME8z9=c9Q#DW2>Ah7ln zkMpe<$wCW=rF~Cme$n~U*Zpqi6d88d39H!rNU&2*ozz&66-aA%_F@&&8I?T_Vvx=l z&rw5_oi~0!Jqe(;WP{@t)~QqB%|2>1++7wARy7~%xMiVKE04SmzsHf%xzH})i5odl zDPTs;O4QJqZVK#|_cb9#eSXGXm+oJXMt5&Jd}7O$Uaa8~b(?SXU6HPw3$Rdr??V-# zy*_ud2cR$zu0Q_Wy%L~+z;6OzfxP{z=Hs)UepM1f{Aayod2QqeQl>2f^{m^ER`M4R zv2W9X3%AWInISt%D=Z!4IDhM=y5*qa8;yGVAFNXSVE=jLps`8~_Tkv|e9PZSXal+K zTX`z5aj72#Igc0Yi&pbdvvKW5`@{l`=(ZqAS??uCY3kJk3O z>01|y5ITO}FoTJQXMMfCFXZ#zothkc^MXf$mV8uj9cmq^lkV(Jb(E@jrcnS@CFQwt z$|u)Vpx>C|uct`&@y0N#RFBE@2H=<{L}!)SuD2V_nl1V`k6(>7KpoW9j|aYOzLY>%KJwAwbbkDBuNm7P+`^h)z3 z;D%7o(I*H3Lfgay5Bvf7<6K?gT9EsFw~_*N3HnyY4(+xD9vCXNfInV*Ku^XzBah{u zYw*uKtbHl2a&zI~Y&c|JtUG(%44nDfTb7hj>v!1^56=HlThZsIvp&x@`?sv4K0mdV z#%g9aBxU;v*y{dhF8zanr4N9M;=p`{p+c);+<1^$BKS7J&XJ*R#= z4NF(l3=HbXET4^GtyU=>)}P6eqon#}EO+wt>CdxwoCTm_cDl`QQN z15%uCJr!EwU3=M`H6NGiq}IA-@j>+)!$UZ0Evx%kWp`vKZ(!{vIFvbga+Yxku48wQ zR^l+7H}W-a)#=;n)!beN-m`p(I8n(SK)FpWV1#_$wEn_%JghGyWd&kasrgb1{-uC} z{5r2X4%-n%>LHuuQC2M2i!|e zI#fK@PxqWY!|}lA*G_Ewu#bB0x>^r+r_=VpZ2bL=X(m!ssCc)WzC1us5taqp;BVy= z-dY|nat;h)`$YKJ#{D`-w996+^0{Xk0koq}jKQnb0g1v#uA?Tdb?H4p!<3pieK6$wK9Ialv@xSFmu?Sp-aK| zm9KJn%^m)x4e?~o05HJ5lbH%_FH8H>&q=dU*M{$;;~QRLnGfW4RArD!DT zn^{yz%5A<7;^7z|LQ@|QU71F0Do@kYG9Opk+&Lu9Rxy+XW=43b10_FdcwoRGGpRSj z^-Jd&S%1vd?k7OEcO>p>(&SO-Yq!|7+_HFSjsAP|-QA`yE$9{5H{{(i&T%qonhAWy zDnPUa+mJS46HPzL^LtD}PB-FyIQ!h-USYD=GHMb|oe#Qn?#A(b3lX_JFNJUj#l0cnS&g{v~HS?Y4r%#DKr6pV#j^q((p?4*7y~nLe_xy6O{-O4D-=6EW z?<%s!dDI#qxq#xt|iQndb#BE{S|7J#W@Gv5Vt|{r>&$0p3O_llnSvKGI*% zQ{-pxB}DT?bx+TQpI2wUtADtUd#15H`n9CQM_ok~W@39{3-Q1b_iFr%R-m4$_1yKs*3*ryt4sQw zIS8?i{KVmPZq@5zIt~IzcuMR{?+woN9_v)Rb${!Z7v#ogtD!nuh(+zoy+Ua~C-19> zE`pjG^l>NQKiwaN&Um;uuOgr@8Q*-Hg!#*{5CIt3A^s4y58NCh<|C#pZ=4 z@iBIdAo*UR3SQVVM)YB>l6*NhYV&WZ_g)R4I>ippgR~tTcIV6bT3}u%sx&a4U1=P% znclQR&4sD-m@!iLcxvpr!*i83c_@KMfq44)^#>`C$~(Boi9dU7XZQYMj+cw?6p5V2OY>4K=pe^5Ku zth;@&g&!6oV1Lgi_NAb-{1KB&H7D)q1$3$jB|gK(G36{EBsr;`B5TkM5PjT z;stcfgR8cwCsiAlx*u)6+{tmZ^a1T)_ejTdN%N&Z)fdd@<*JGGK`CM9&GZ-R!+wv? za>Whzh)$=5rPnN%gB*us`8yv2uiE8#xv`*lEFWvML)w%6tL@k3O_V^d9UnF1jRBHo!_ zp$sh~p&b;zP;C-aSv0dEd#F!L)nKJ`5!aZKJKeaJ-dknsms5=FNbR~~8`j%3PCd5% z8AG?TJCN~?gCnxbWjFTdeYJrC!l7;+F0@0qJ>?@#{MN7@w8!@Wd#KWcY>l8bVf{Ux)M|goN`SulV7Z*aU87%nBmG;gr_w40| zNk*1u7TQk*3ADV{?!qoPmH4S%!tm4fv-_>thL%m#$(^-oe!x?8q3Sf^E0i*g^yJFe?6ay883kEH9iV-m7ka2NHLuT- z)fNb>bM-7QP)X^ZQ)bLx_dD;cWNs%lshJ0|jW{>6f_JD(x1j!Cx!(#6X#4x+chv!x zc?Oo~Jq~M?34Zuc{l(A?^G-Z>(f04l3qUc!|1$)S7*jY{So-2qCela;SvC$#p@2zSkRu?jE? zr=4SweLMqm0j}4-V+&TPX>-F=!HgE)mgFii_W+7&N~%VU1xM2i-=BOPqJf)=cfm9#NKAZv^0BAN(H zu?4IJ!D=PRVnVx%L9q=5GH!J2u%9P@Q3Unb@Pz- zuhn=%0Zh3_9NPuz&ULh-NA3tH){v2UCo(SswKVRZI$pfH&SjEYVPX0>t@4BVSq_dd z^DxjZbyU^$k(+4+w=L0-UC;ee&F8XV#PJ=5=~;9}VRduX@K#{}ft?|`xXiwLRfiK( z*>k&8_w92HG^UR%zu#Mv>#Ru=Af~krRk1376qGX$bd8(zEtp><-}>9guVwwU7&*Gd zbKsH=(H2ffX-OV};cVO>WEH3J?$m%Z>biuvl3;)F%ZH463KeTva%7KcDvq1eTgbg( z8RsM9-Z8Y?=vNJEtO)B?>U?vh-_ogV>}aGv;dzAL@P^Vt+HB)h#-DDP<(ZRF(YUgs zM1a)`geF{4ScT^MvPWBrI@uv$jv#-e#l`29HM`Ltz^Y>VlmYKQ`)|iQ07TTepTv1aeZNrF(TQu>i@9CL9wmWE zLNnvivzzOs8@4a)@!+7d;L%fixJ)-N!5U7qHH<)++{} zoD|!m_}DY7`KS&wqRHp>Cnck)f$5oDb7N3LY2%xgbzzdsrE25Qqt z6(T?-$Or~*IkgDPm%)~E@<(d4jLQyQcF_KipvXCh{Jsl%{4aouKe%@+2+VR|rWxAh zqCDS*=AqF9fF(~W&J#m0CmQ^D4y(T%@A&ela@7Moois zaOdgs1%Za9Q#Jj+r2O8Av1>(nRfgnaUUTnD{Eq+vj3S%hGe`lG`+}aRW zy$T9$0PyFJ)OkZr`^OsybptTrT4lla)@l3udZ$t^LkUl4ce)VwI{k-% z`-iq&=(-NAVhjiBDtLDddgLNWp98~oR;JZ2pdf|BYSoE4|M9xG(4Qg=V=f0#eb9uJm% z(=zJLHlNLrD!k%qF*tf@8hF@`3l5ormf-MS*H-x-zm1p`kKZIW zB@Tg+jPt3&CdRpPGJvX;_{{et{gz+J$iM$TTwfBx`_!Rm<0|9MfTZ6umAu} zUB_N9SYF!cpnMK`z+*5PaQ#aE=|(W{nZnWoxLgJphya=lhA5WQJK*YzpkNv`G zBSGW`2FcwSyF}E%zU#5GF{6~Vy(-$137(se5v&^WI#;&C5NL@~k7K=UP3iea;K;WB z8Oy&Y1yY%deg|>#_OTPT8DP+3P1Q>Vh#7QGv-i202a~HB$A5UWf0e-o7bhQxcdI(~ z?U|b#RBcq;Ae!gAbN_+Q8A%5XGMmpy_0#@OF)4V5C))eAi}IcFsD{j{__SHszblU zm@WHUz72*gCJ$zf1QzQtvu(xpC>H@JC4TvRM%7%gQ5Q&y6ZR;7D`=beGte)21gtuE zENH!CadYr@^%{9#v$N_`+pXd&wgAfovznMme?xIZ{>%!B&~x8K>v^WLEf>Zt`!LJ_q41Lc>a%o$a4~;^ z#0&$#uLRIc&{n^Ystn|Jy%D&WT5;$d4?Ul9h__%sVp?zFlrJ<=FUy1jus;(ZE)b?t z`QQ%JVN>r_W1#)lhZVJfUO?s}W&jxc_S`XMsX8DvZESq|ZQJ@bdiPP|no=Z(S-{$l ze7kwa<%_o@t*OYi6%2~(9mQWAmpRT_=<@W>X@73;bTb_jlU1YEV|&V?p=lSXC7u5X z`_$}@8yW|LuPREjX(=x@7`+nmQ`&qX_D4Z zs*47wvCpNF_S@G3*ZZZk*{nv)q1U3Li&5*UK=W!dHcn@4Q05*AmlkP;jx?boDFG9(*QIpBc5ZdbHlbSGMX2_)tY&mplq01d+Gm9Sj(HBzDcCO%p?9775N%h-k z*|fDxD?(Xk_ITz{hO&yzy-)#?>;(5+LU^MoBvk_=mG53Fjqk$aRL;OgqH}BKC!C78 zFgy0DTBP>pPB@CDLwhYyseALW86lnVEs7hk*CDHQeS0^vl!qfQF7 zN@S+A`ogk*W^4{sw5*ug+LO6r*fPxa^fek+9j%I{p8D+lF=vM{(7Fdi(9DO3zQs*gk@~f za`+`0n>dATI>ltWB-C{%mcY@yJM`z9gN}rbw3v|)!S?ed8EcuyyiQ8Gva`ZiT6p>t zDTP0Mx%aZLT%q&(=VWphiF}j5-NJ90=qSA%Y)F4T4%&-3of-D!L_e=j2`W9_hEUd@ z4eVlUN)S_N4#o|ow`Z&?6&SSI3Yhu&#R2{Rn<~xdSFxFDNy%L9byc+fOE~5j@oYxz zK61grUp}eY_YTYw6x1v}8wk{(XL)P-VjBk=ZZm59jc$vci*%dpsmYZSDNPpg$)56) zc|6!IrJLiK*GrY40XTBoMG)?4;{jatr(m3vvC+LIad8!TAqR}uL4zQ8ADv}bFxz6z zM_}KyjbP=WlBv4lc2wY>-YIHa3u;zLpJJns-(eDm&v%`SP6sAtKDu`tXt|w7 zJtrUB^E=k)-2|)PChE3>=bJ}^-7!;YMd=R4o9TP$##Fl&G|-m4SIm^ZVJa6gA+}}E z=m+p$+;i_a@mJbK%b%+_u6HnM?G*yJn~TM&X1vFIbLMUy_@*N(VXY*+kY7n|<--~2 z!h&&5B_dI0xn$TigsRwd!oJF0<4$7d$!M!q^hf)i%HAI#HoISdi5Tqs)uC0-L*;z>h9qavqzuZrA#JMy|AgxccQ7<0l7`s3j@w_*t)22Y@9f0#hFvc%tFt}T@~a;fEWIs~?!WXRw_GXW zr<#=z(cYzH@OwsWBg}P%Z>?7}wC1@xey3w@*ypsYlkJ0k=hQ7{*1Knx%fE}3)(NEb zGSWS7cT!-7DSZS7&4jNBG4-}O>9J$M==};8M|&^dw!=Tp6pNYiiK<)JcOWIiX2rht z#Ao*5Kazb0I_~=h8=!%LRQqH5wnA#aH)8f;bvDSrP<%X_qmq4rd3i&bP=m^t2oO3R zuGeL7!8>5G28>gH6qzp^_){?Z8Cz4(NnzN^o%52tK;pzUFE@6Vt$9pvv-{WIp=K(( zM|0otKNP-piFo!b@$Ad*^^IgxjjT={c+e zud+l~iudwj?X{M4&s|-EW@RCH{s|0x*U1D}+6emVn7eQZwSK{y*?U;X06q!W9~vhHS4yMbB!c&z3IT?GCF=cVi=Q+nmMR{Ou5()} zelct(#PdFl6TBvQv3XtH@_Q-LF7M6)8O2_D*0?gp_1@R0+fgHOADMUag;?t1u^4|_ z)N`X#S8`x4{@%Bgp{%NWVs0>((9?SL)%!!{T@x$(sXfpAul-nZ`zf9lHF~1z-NxNp zd0kC0H*h-6wqQoQVsQnVsi;Kk?U_>>d;M75QjI7gve{mSl*qZikWYTLVSlIbrf2iS zk&{8ZrE?!2$iG~gNEcgVAt z()9DxC&9;bEVcNaAIW;|VBX(?SRt#G?REd^rl4ZnA(LS8P1}{~ObMYnkV2+{ZIZr3 zsb*h!0Xkk_mM#kWZR1svpr!wg^J z)9o!>L1Q)#(Z6ftVE$_Jwht+G;hDjlSicLJ0ExVx^;rT;)!PDQ__f5T0PNIeS}^`} zRyeS@1NTL)Xzr-hxRq|2<)hAv)dsQW@-4~7*FN;8fvORRPZyeuficR#zVv_Y24la_ zSQLSFMf|?WFWp?m*oj9ul0m~Uv`X$Jy-*x-Ma|5oAnNe&I!=O13<>|EiO}*volzRBm%3D%X zGK{4r<5*@63)E$qaF%wrBD!%qfTNvjmzlIU#s>-dJp%e_9Ql~XEl zE;}~=+0Q%cM=Xvb_fPK^vWJ}|cr|x{F`7c*-!Wi_{8-ml7U5^HYWT@+@IECNLHcaJ zUnTyV3IDvhWpYjClS6S~^5&DdIlph^Le=Ew+P&E!eY|Av`-0`=xnE`mZ`eA3c|On< z;Cc(TrJvRdurD5*51KKaLRM{Qd-kwExWl!2lem6UmpR{Bh2+h-PGj5UT>a1&@o@fg zEH+oBs!EJ3vEpYGAaQXsB5`Fh)_-2M7U&|#B5q%JHGU%Is(8<(;Ehxw;#kn}ZC%fs z8_8oVgBt(vO4S5`4leE?6|+jU!$?Lsd1T@$&`K}^PZPLde<&#Nvo-g-p+q#0j|OoverDLu_0mt0*jV2@yH^RrkgPtp2GujcmpkS z_R@^&b<>E`S)UkE$G4$or|W~%%gZ_4<_piiBq%Jri4}jEW97ez{xE)Gf!8q|QMkSmIya)f}lzOj7|?gE@^QDQ!> zpMT?;^H+l>W>JnGSX--%^C$p1~gFx?9EQA9rH4o9-WaSb09G zGW?EAxajLrn{=bp%qc?2+03|tg{aIrbYjYJ|Bu5SO=w@3%`|!maC?IUmos)PExk&u zbLJ`c%iny}0OfI;O)yLR^s}=Q@2+#s2iA)vMMJlouTjVKHRmM3c9YbnSkpQXVZlQEj_+;UA{+MnGiVC{CohyMB?k*${Eb3s33x0r+c|@rZe3Pb~{G#P-?7k7JXpXI{&! zkyRMXRXRhyFn>1T)v>##Z|#)lp1r>8z!1~-IME!M*KPB4-IhO!&ZX3g8A8O;n z1T84k94VXwr%@<aC?h_gGmr2uBVs|c5VbV2xmlpOy6xTjqr zTCQq(B^@LaQN&vT9!T+Q3u1c#Nu zh62&yvnYGV@4CbpX2rn^ccqoohK8lEHMgPrLgCBcN-XM#){nj=ho8?&pD6SIPYMpo~(wiKAob={hdiSsFh6U(2D=0Bd^7*5{%@p=XJ-5v}S#Q7Z)U;tI z72$eLQvu)(N6Ce0Jh*aXG-;X+!s;ObvJp5C3eLDeg#~be6)MRmJR(Bn5m2%<3>~>P zr@;u+nw_nFk<(CO&2xIh9nhU%jYKt~p)k8|dZ?jLJKObtfy4uR!YM8>}!}5+r zWR&EpDYL#yB#JNk0aq&r9rwtsyO(b#gxn;mdUwQ`yt68Y4`v8TYuFT2=D6oH9}nfEW(@xgX&GFTc|) zh`rZeyobfra&f^(r@$nFW2u{EGJj5<^d+u2k3CzV$0u4~uHvKq+Wm3U;!7T>X&Q0) z+iMrU+L)bwd8L5mobZPJxd@j@dvB*wX}#3BreEjGTk}8V+8Pl1yCoN;udMkOQF_%8*lIMC~yS&OepMGzo z{Mc6;a~s)(zWvw41#q^J6L-k*(XqKAeWHo+2mH6at4`ju7d(Gk;?c%VmA&5V7=~y8 zMsZk4W+)WhlsF~a`|}yPSH#P{xbz2h==NHQd(@?L`bp=OwL>jt;d)8EW8JZ9X5TC~ zQHy&^tULNJE7397)+ z;m8!PF>y1Lkd*g%cX)DSgriLQp3cgvA+60fRHOCEeY*bQZy#h=9bY^+WV)VJddP90 zZJVj&?$6wKN%OqtJ@RV2Hk6T{ieo=3Gmgh$A90ZVTJi_tTk&}xS)=#Y?U1g3VfabG zo(Z97nOeP*++h!GZs?))TAdx0=Sc1%SXAclYQLQomEOp> zK#~2BQ~#6qxkK93C}Z`cmDXGj!7+i}^1jh`&nJeA*FQ>gAT#9%i#_k(ATwmMQ4DuJ z+26W->5{B9Vm>MTl6>PpS~C$ayWG5dZD|C6=hk+w-^D>b#O{Nd5r5#dxSy?wzt#MpM)$#ueBJ77{WjdUM;~R1>*jSAMEq1WW2w5s z>dAejKjpovH*6ixj^%}^3>VkVJQy`CHzXZ$=pAL4D>+Z28%;t-_{UZ$5gs~q1J%17 zE2Z2#%dgNdxqFtOElDr+Iq_BBLtm5jV+MeR{Q?Oc29y^0F$0QkJOD-rdVpk*zBvZL z8hkZN$n>C*kghvAn+F%~qE-JK#hmTw`EjzwalF zC3VChf^7R@bh23A*uUES6R+)5>vntdHt6=_5JsHKair-O$B04xkh~*3f1U3oCOKiG zlibfRNrI*Slm)my2fM#liL3pU$=f&UJQV_oSyPtdiHioLZDEel{*>xeH(wryfaR<4 z-cGdGGJ8f?(!+L+Ufzqo-8o<9s3QSf1 za~X#YU4xwWNblRU#+(8r-5xoY+wQm%R#JLvmALB`T>Eo8{s)?YloZZ0=`}{W#_5@p zp15=cX@P{(BugEs!Vgz%xyNyu8a^cerN+urT z9i3yRH<94in%RGlpj+jUwi+vS?~aq!ZBjyyNv<1h)(8;XFcAd z9`(a;Naxc5ly=!IeZ9_pw+f5&}{)dw7_a87<<)rDv>UZ=_>bkhu zE?hb(DZGwb?`iwaGx2)kIcX!&3>!CME^Z(5JXw(_R~z;m-?iZmqdP= zCtbdD9AjRe8XJ@4>X;qB54*5sEWnP{{qFZFN(&{O5%Cg!SpFY6SeIYGgnee+_VEhM zzE$_d`LHBJH!jxc_?xbT3CP5v4xW!8RqaBzV0p`A|>G71(5h0~ncGg)d zvBlbmV6eyh+r;0pYW8GIDGr;z*LVjw{9(b8VcURDMS`ww-&s*c5~j*LhwZOZEKqu* znWl#|`)5|(Ar+-tbW=<@N@rHHDtNT(3_zE;))E4-=%aM%jr&ZaQ>Pc#6*P6W9Zfz3 zbO?^X-EBL3PkP4K-MyOm#71`}YgL+3?*8ZiK6a)%-fL6WT)j*4CApM0M)#fhK|wdG zW?_hB%6QH>F#o1wQhMul&mD%!YVW)Qm(-)@F#IR38;*1E4D18 zcs6#UZ)5#PG&S?&6lZP3?*HZ&T!Wcc`dv3o?n5gl)3)z*%CRNe=)T0tO5Z2`dgpos zLPhhFvd>NUFeabNQ$md|-g<0ftG^xpE;&}CCpx-FD#gF>08h+J<;W)gFy*6XWS*ci zVTrca<0*JxdGSU^%B*NF64Rmc*d{aYt7F%TN|j0=9!Md3f`AKX^Vd3m@|SyEr?a*a zAf}OorhYV&lgG`K6#iiqp2nP{({N2@^WbM|tJ=|{J*3$P5rIw};yQQypy7vN);WPP z_WunV^)Gp~XE3OZr~zFLiD)2M$06PC1#3n&HPH;p%pv^$ zXuJgGV57gGwLI^r#^wdEZmfpVgshxJzijAi67>(cg}*0!o>^@6W_Ol z3Uy_yX@`9l?g?%`j~eD^%*&bQ4)jl0n!PCU@KI#Z78;*;7BOF@sK6FGRk{4EFy)z) zVp&z`8PB8DX`UZ}M7p_s4}CklzY#VEZ-kc?epeB!{(c|UEEnwU5Dkat&};^F6K5jj3Hcz+*~_~aJZt)d}t zBEL!_d)wB^$6$+vm{ZU66cS#C=JL$#(l(GE<-K5Q=;}|a2G$;FO+FTi#pRFTzpzE32 zrq^uo3xsfo?%QvuWa+B+b{b|^vQ6KEPWHFO6x*Xfg$e=+^i(X{y6OJ`(297_>nX|(fw6jx7I1rDBNwGryftesLmkMZ|@LOqlunpLxjo%=vjuhWB27ym#$S8YA|2Fc?_~#t4 z*sX7wcCuqz49oWJ9k(=$eSKA&3t%nHtLNf8k9TaFiJG8cjOsuiJmSYD`x?aNsf6xK z^QQ$NVQOMT^u%2Yr{$xEmJstYI5!{eVzdPJ@d|g4}z7;3?oht zf{d>OJD+6EEK>DQ-kWuN0V!cN^A_<{4`3GrM_Z0qneV>BE6jm?|FL#4Vjh@T{<1J` z(-eULQD*K&?T^`O!M_qzpXD#MQW755-*OMC`XKa{ zSbYI)p%F$SFGyh6RxdsBRAQF6F+0?+KyToYYH#d4YGdtJ0YrCBK7eoTHaYNBl!?z*mgj5cNP4Tw|}nlRmayO=vnyyDZZra?!FEleK?M$brREH2RUhR&U_>+ z@MJeU1~1rUg&Lpwh}S?F_h#8YKRx#mI33kV2R5wm0Fa@XYi#OkA(3b4_MbXgWweJl zHk^0gV`kc_he+tQt^8<_b^y-_#S6fV@AJY@&-^mX0_5(}99aM!DO@zuI0JIH4$^#cUpP&@)gilqM^~>d4Z6yd9-!G!H=xiS8n90nOCLm$Tp&WhKkTx`D?I zg!*92?YahU%td&K?{74df&!knkO2YM1x-^rwgA8V zjjS)q%0=_Z+zz~ES@ama@Sytmf70MSzY>?3s8SrAAb--4B6$Ch z2u7-ALZ0E_{B9dYaRDW+oE8uzGP$g5(!-_^7`mxr81A=Dv>Yz~WAlwF%OOtAy@L)m zcj$A(FyaPK?8`p&lgI??RA#1cS7Da8Jg>kurR!_+%2M>Pfat%@cC;UO$%#T>?*C&c zm^c%n9aeZr>Z21)_-jOKI0&S9AP?EzJO{Fm`Xj&)G_6{YIoLHzw>{^B;f&cUAU|lh zQ%wcYZ@VH`8RWomK?QHt?f!kZvgbbcD|`(*xyZh%WB*AL7|aD|&P!SqqXL_Bz`v{< zHPV*@BKf`pD!YYH!*=8cgWL#Ky*cWpvTKgN!np8XKok|u&pLUTII=Svy=)RcW`F~8 z#rKV$fikW}Lr)4w5#j>j*2;N5EkM@8bKXI!`rEq}nXOvuxM?${bmb?{;EYuT7mvQ~ z#80z#HqF6*W1eufI#sJ<$?Zv~akGas87%{n6F>+(klay+=hm^>&7WTFF{1zjh%(@d z+{uJ(e!4M(eV%1T{Le@D<85c9az#KcOrAG8Q5alwviW1uru0CnLQE#p%p z)3hLknw2owwnTk<@v89B?(J?6aKSE!rPe|P(GKQ?pqdwl%QTme+|A#b&3Tb*{d9k1or$;_87sEv5&oh8*-kS$Al z?!c~u!w;wBwCs(}v3+^EeWTAeM~8zq?M!w><7#Vo?O$tNrtNQhpTH9Ed4HqQ=L``Y z>PJRhr=+ZwvMCHlosnj{K1IDqx4c?$V)eNQk`WUD=pTFt!CG&|{YpQ*$&<7 zdHc>;QgzBp$FehS)fDWNJu(}iqRClWVIh)3qtb69Y+Q*BCl}?iU%ravJ{NnRt;$k= zdEkaj=0r!WZv-j{8TSZ2C3tISW0zHbZ1xWl+e4(Z?#(5PKrH|ucDm0UprEC9Jehi) z;}4|+8Xi7iRv+4~jg;ZnU2XZG=DnTrM8RbUZM^CyCL~qVzS=s>TjjTE+f_m4LsouK+2as4ZK$h}4HG$1m#H%{V#3dHs8){1@a(>;(de zw&$yY3Zitk9QJE7dW))RQbrHAa_3_O+TpP~tF^>NpXt$g2kZz3t}i94HNDNH-~+Bf z=%{~wBD?tZ5(Ag=i$nFB;{sCH9qAGdGIAD3Cy^UhlhfJ4-*_1ZZ}C=FG^v#%glAVGqRN+YS`K+jruoKe>n-YZ zmtrMdNwz2DH{V?q_CGq4{@Hgp%pKb`ll5B7(5K!fSrh%?SJ*YFkA5okbKSH{msXQV z-oK-SCNc?(D4n zT|PH!@Pdl2X2&gHu~w`5-%VL2YBlEt=<875WDbY~>mp+OcH1094 zwLb~TX73w(z;Gl#rK`%Z*_Exz?xU|ecD_=bCfSFTg{MA9VuOx57bJ^b%{GHuBx)t{gd$Swf3iq60Rpx_7`g>CY4vKt&1+Jp@1DyHevXyb&Aux_$b~+_Hqy0;RzME8ZH!-3hI*vl;BZZ;F;? zHnkruNmV}d?pk;yb7XNK(@hV1QX53?*TTIb%(sw|7PHE9>Ft{l&ZIJVQ%h8X;DhN{ zo^#GO%cfWLXaw=Ja@iSo3Y+;$1@wJsC9bX@NI5ZmbZ1QrD3^AfVm^&wt6Ra2T_$`k ziNcygK8fQM02LFBO@l2 zQ=>Ul5%5U3rwe6Ds;rBW5<3Hl!8seN<%yYV*9$-0ImY3{ObAMk@-bcX+MaEudU+rx zd&^L(pPnkUf8W|u2Z*z$k`KWe_z2GE3JKkNbGPf~q1vz6V-vBMWRskiE_!DlU_Hi; ziqEa{53?InGiW^*GYu#G#&8)=7{+kupG7a~B(^t+V?Cozer>d(ZzX(V3+}%AT@!k9 z9?{ATNcQgvSlkrLZxlk!8=D90Wlx}ims+Qo}`dbz`Cq9O2s+(Uz%C3 zWq$c_{jvdRI^@=~VcI)$kNLfVvkfve5z~)YXh?q9$+Q>AYJ?S3&)LHWhr3&Qmn;ov z?oCAtnmdH*!@>LIUES(xTASzmkjW3!yB4mt8xIqGYk9V*0G!=G8tctiN^<<~fKVU+ z+l&ydPg<(stT81heSYy`s&c01LfNJL{S=@S7dI#jFRNEF&{e?Ucm`sVacmV!DUQj_ z!WeR%VRW_VYQ|Xd3yY@}SF&)3oI2dknJuqiO2nI@Hn~_PT<{l)%IY>9M!bHt0qlKh-@4TcBl{f z8v#WNxYLa}$?7SLBAA;LzvbyHA)~aB8ZdNvt3^kN%K|XHd?&IgsB73~PPI3Zr`)VB zV_dho?Kd4{|B^v2e0#G%Y%ea~9<#7-{-H4z-Rmd%S(nO3(SG4rDg$ zH9vlFVE3rg#%gP&LUg&v@&m_pd~N=VGt30Rv@7@LgW;#qoUi^7t`X4zKcdkjV0Sh8BINWPnaW0+;w3 zNsXepSwBG!8Dr{&P0Iph15gq8l#l-GlN^x<`I~^@tNI;lragA9qay*s+KYZ)d@6tX ziP_({IveDucKWRCTxMJn&ufr3735i}d;f|9E@&6&TW#Z(V`x=fU)=kcx5Mc za>$z`)Y2Fi%fLG7XJom`yDPcQxYTR<<9LB~QJZLcYKpF^akwdS z1OmXDjJ&{$V7(6oKd?6~W$ueZ_BY@~$1tgBoI+L$bhONXv-T#-XrZ57H8$1h6(Ylf zmQ~-PB_x3n9lKCMjMCOy&{?e!6M8_B;qmXWre)s$U_ZT4#;rU24%leMWxvN~R=!|; zM%*6+sC5YI0OP(TU;_=XA{MC)tv*7whdQuOZ$7cj+ZFa^TaM=Nsx(P^HPG3!E{yO6 z8@*aVh;wY`ynxOBOX}~(FMh=w zwN_zPci}kW=KJz$d#&=9ITQV#;b=i%j?{+MUu!Rpqr8o7p_*>xca;a;*p(mU$T8MOdDqt0#KL>x!b!ouPH>h z+#MDfTP~eoSt`}^&-?XsY%fnSql@S_F89{F0&DI$xHJ%`t6oz=lT`g0!U!b{n4xc< z4a=VT=6txsQd2ZSjDE7XX{4GEZg*vxC$(~~q(k3Wo{pAs^5!58q%c5{f6IURo{}5P z76GWBy>|l=LEO#>3>>W9cGGD0xwkZCb8!%;|Ka_q5fwOn<2B~MJV^Lyt%2BM zE?8Vc3?j7IqR>G2a0vMC#+}{gUq^X)e~mN9 zp);yV8oLH^2VMl`?^a>GP)<5l*!(ki%U{zK8k{rQEe_T&ej>hRnqftpIL`p8t?O&uZVyDwQpeO*JJh^WKkS^9&(PG!>1i9A0;VQ*YR7**UdT-+%~J0D9&rOl=WgNsg6db zkvdU6B1mi4b(iMFIy6bXKykV=ac-POU80>(oyI1!dv6tU6NHmNzFr^(Do|9)5?Vd7 zyGZT$SZm~*^Ez#uD}TH}gAmR{FUxegn%`0useiRl!+-7BiP2~KlNAu!xWyOC#XO2qnghCSwf zeTksg;0_*GHX~L~+{~H&Q9NKNJY6=QHhe`Z1h&IK z_p6|^l!Csc@X^b@%igBR-m}1aY~@tH%l)Cbb1{VBjnAehRELvBIM&wp92DA4E)Y8F zT)aI<2YE9X4AhC*Th`^bl)CQUGSC+2Uq5NZFIC#UCimdyMEO2;J*=DwYwwXatKLU~ z$q-LY%yw4wKiN){u<2BbOuLa_PiBQ(=rNCS z`V(W=`Z)udKO~u#C7I0`gZs`huJG{BUv%yXl6yMQs)+~m+Nz;>qUHCkZTq&Qa?VEU z<6fMoahVC8Y-NMdQ*cPg$ObclvK>ZTV%qVdQe4e+EGQP3n1}7F-pD#Ji>7TQr+uQ=q}g$p7hND=W-)$94mI97p$< zqDUU0(tFP>e4;B|@=P0`H2HXPefE;uk7!RKWM|lYd#8MDdtmq0uUDFV7b`xkPB6eg z0TW1<#Gld4oTgb@7yY;I=5IYCEsMV(xW6}ZG4Llm=g+UYH5XBqhj*i`P$A-@&=Ud_ z+&-_VcEel~LO_d6jDdd+?0(@RDk%_x{XM8K!qTtC%R2WkBzy{e&9c^>M4_vtQA0c5 z$PQ0TOW270o-35NJtbl=MLShmhVzPbe!APtn4Yr41PQ|xQ2P6WfBMxS8~>A&VN?px z%vYL^=nAAX!oXozNZMeC_I9s?aJY3c8Pd!Xv(2&lj6m?+#rZdGzUW(edL>s&l0Dhk z-4n`Y8x!df4nL+n@IBa*s$ZQ=-Ze&UnuMu~eS`#Y_%dxv@sId$*ON|dta;$(DC`Qi z{k0sgKy)!H^GXmMk%c!hy8BObhinfpve;DEpEC5HhrMxpOQReG=~2KvP$BJ7u*p;) z|I$g6BL&uOtO_Z`Aqdi=m$8U3(}==&(~2tDdFHFk^kz)|fia*R^tlF20)6WD?*!i9 z>HcmAALw%g#N)r8{;zclJaHQ9NVWv0iFvk-0Wdd!UuQHJo)7XS>5?~a(8|+dYt%Xe)1JfQb3t1o95|1L_b{wctHyMy%fFQ7@kPv0A zzZ{Piz<}Td1_fCG3RigR0ks1>_uphb9t@zB16)8f2?h{7 zh%#|R(#<_vHnjO<`yFm`>JOuM~(+Ti|=m?5Le-Cf@Gve1>k1}@Y@IaB2{?)G#>cf zr~w)=n}2!|Ph)mw`1kPL81bVmN(|?K2-dqcC~?t2 z9K*mFSzjzF0wTl2N1G9$Fo-!#bWWAie3edxsfNi=O zxdyzCG1VZzv#HJs!0ZY!W8}Bc&+OE*-xj2dq1ldlbfmnko2a5Dj%tu|9ploz^Qrgt zp8pD5HPC>1e~4YMQ%`p;KmQ(ApNDYpwSLwTE>`TT0BH%>W}%1e4wlBG+C5}lD zUs_&tnab%*+)_l-T9L%HMVm=mJC@42`4P25B-+FVOH~JY4Sf0a?4?@MbOp*C$|mqMN_kZlLhc4zuCMISqyx)+xmRn+PBv9Vh=)_u+9N_NJ%aRC(xzfbQ6cTddmaNxe!0@7xSN@6F_SZ#YlA6-~A-{IhDAILe$!Xz%PfDxD>IcmesyIJV4ZlIzxgUqH zefIn_&+kn~MeFSUHUXps1_#?~V9gYS@M&x=xQ$#FnF7MGKqu1pvxpO_Li)IYiP8nR zq!pZV$^d`JDU8*PGXyk`!>5*3?-BlvmBdTr!V|MU8f1JNCfd&Q*U=~fs@+kxE+S|u z(2f94$e8Q~hn$EdqWkpt5(m^HHP9r#&wkFnzr-TJTgN>!TO9!bPN`n>SvPHKI~y@m zc11C)Rs0+Dhe<4S$PSL(kv`6+1rXMz zzAIMj)18#AJG11Mo3~Pvq?`+ zsGde{Uin@Tt%yDyKS2m8xb^O<$5u51*#7ws_=DGaKUdL;H6quhjGrnkZjsRCI+1HD zt?Aw)Q!M7jWecGjh1U)=g7~zDOqhnkOz?-jid&~yP%ampnW3s30G12v2*NH0Tvv`; z>bX_pXVFH=%mNbAH6d-v?6X#Q`ndl*s6a{)b=-hUhqRzbE@o}B>e~^4o#6#cfB59! z-g-c_n}kc=Q?b-xob=+vg3o#os7kssfBoVz*PEWK9=T}b=A2vy2)9$5fcZdLz3U23 zMulMmO2&!&Q zB{&o3U6_hp22A`se;!xuel1}m<>qbE<+m<_xtS!bzm<$WSMSpbRM3yoQ6}#sI!wE-DhSb#hf3(~#cV-6}Hz_Q%y!*98*)v&MSqbbH9j_gN2@YZZ82$tJFDQ<~g(@z%xNzVAdx2l~NU|Jfk4T!Qe*OCO>+aWIvo<$ZNWQrK zD{GnPZ#45U0RI$E^lJ=0eu`Q|t`gXwmO*WkT$2>9nbfkVZIf%KbI@kU&601CZ`1Fv zFGqfc{H(xv@-fW|TwunJw+iPtg&u9GB-8POW*7s?;V;nLJAR#I;m-=59G?4lqQ5E; zYGjaa(8y#VMmWx*kxfG^*;E~61j+!)3S!BxXmLuG9?h#ukO-$OPRVn`J-7skng*y}~6di7HFN7zT&2pydn!Xez8Y)*xcK85bv;d96HJrr|sd z7iqXmhbH`D`OGdj_?+9g@^kis^D>489(?|ESHlVDTn3XN3&WNnSj}4iXYn_I^9-{G zO=x)Mgk}u$HraIkUwp90^nw5^41_S37-2WD&jQ_v5;JOsiCJeT7zBy!Y3&V?tPY|h zuNx$$--)+Y61!h}?&-wt$)9?ZsE3LAJTd$@QT;>>5<5N$VlOefQ4`$aAW^4@dYPzu zu6?i8!V&5N*ZzPZ*E+2Ad#-(-C%-5CEy(!w-tJMS9qhI}T@PBV&hBxqJ3s{0UPim^ zZoP)dQ7078589pG`f0yAs9|NNJ4hBALCm}YPuJQALBua&><1VqlTq?D3Tpgi$XHbs zwayQq6?}QE4E$sDfuX;Rebca=!-PXu!Zv~RI6M_qIp*+_0RBostXe@h^QxJy!Z|Ll znimm*NM9L(0DtRnM8rP37PeawsWuJ%D6uT@1%~Sg;qj3O0iIM?00gYf<8)aNvfO}6 z!+SHr*pzsMt17y|Sdo3qXspi#KRGH@qrzOW!UC}d&lhip-X|96vH}NWq!0-xl>(C| z;cc?w44o86|4b>QDRqXlB!o|Xks1WZ-{k3OsOaf9pdb(ONlp}F6f&$u8*CY#A=P~| z0ZW)>z$pjT$byBC4|zKzmwZr#!Er_vJB9@bc_arURESqf1J*wKS_1aNX6Q!_9NF(j&5tzT}lccg?`XK(7L1bJR^*5Q*+pC%_X+jBe7Uw?KG&z`Z}oe4IVgKq{A)VnX7xPdM(iU zyO>3r7$|>J-BcTD!7$ZZ>N{!;F}`6`RNk03mQ~ewTfJ*mEXUCAgSO6@mp?4Vv^;-; zJPI%8!VLF9VP4_Fj2esJfomZokoV#3=n3G=>|_X32`~0V?1h`b$MCn$vwS+^LYulK zuUV*Y;)z^G4f-WIfFhxAbD4%?f`ThBCV<8@R}O*8f+0sdCk_-GoH);Q6+a{*oQb9V z{y!;(^a!1l949<@_7@tVbj=WRRn1d{BOV1E`e6o2lvj?p3Cq{0K$udYRwA}QyEIW? znMSZwSuB!f9^?#^#YHAyGhwWpSWs=)u|l-!46D#Sp!!0#h>X3 zG#{;Tbseo!oRhSHRCmc*mB|9+5IeC6tJMFo0b>D!>4Rst4Ym?2lan;}TZ6^Ba%A=5 z+OEmXA&tQLQ=46rWHziY8{8yr1}xdomC*(b*F+H9+y=KlY?&fc;rucz^+V_ZY6x^< zkS6p0EI``1y;%Tuj}rvkm*$1c)fqL#fztAG87-=!nM-v;?U>bVs{YkIIPgtd*wfx{ z2baB+&xH9nlqQ}yTqnMR>9x$r3%|U)!rRO2pI4~*s)4#5#N$gdbjl6{{VvyTu1-_ 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 f0abe3a1770eaa6206c6a913f3ea345a04f59fec..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3787 zcmb_f&2JmW6`z^?;POkPC`zJa$Ju@*QXoii+M>2eq@UORPC7iW0e` zXO}W5%oYVy9|IUY7d;l}t%n}+XY?4%wU-LL^w3LlX@4_INmSiqmzbG1@4b2RX6AkI zyVYvxJuCd>2Mt30O6EQr@Q=}wN9Z`=v`+#W(Idw7&-#KP62rsA~CwnJ4~;`w}j@Q-9tu5Astz5l2wu|%p_Yza_vHftnrHBHhW-Jy#tLgE?}%-tk0-MI_8Vm&ylqz$?A~d zT+>EtcA;xUR?4}qOwKgxI%o9Am5jS*EL((>MerA7Rgw$r8Ea|H3$i9Pr=>YH4!bqv zf~?ENfraO5oH`{R-?@Y9Ki6+^-bB|$!}piF0uMsYO=oFHo6{yUSfO7Ha0$Z&T+wiO zjz_P&;F-aCnOSuHZ+swXeuDsP1QxWhK2>Sse{U(?~g?5aCo6M3fwc zqexkNm~Jj9XVQKi3gwK=K=&1Wpy+3c@l?@?qEqFhhfx|TYn1dLEsYdCQS_yv9|g`w z?LM}k9|z9I8Vc-#_GBD5_w?kCM0^K2e!aVMI2=SfgHUv$et)=gG#*W1&Fz=T&S2DO zV{$l*4alRxaHn%J8BN=uY>%d@-i=Z%D+)z>0C(u?Ye>F_j<_~8KjzWdPeW{hHfcjQ zK>DX?b>Kgw?=$f()?K<#6%*}7#)VFCdvFQ5M&Jlkd-*H4z#3_hi~(;Mymbw4!BF5T z++HFu6!fbw|I9WfXajPlztys&!>v`KW#zM6Dq!mx1+$#eWpG*-QY<)-dq&^VPHf~D z8AivJmNi?EYu6%JSHLPq7O;wOdTfiYq?38Em+zB#Pe{CBlhi#fy{4i>5dr^_ zY^*_3S%L#rq$kU=VjRjj{n|LxC%u>_J`BwdWA4M&{FmEeJKgqSf_|qzOrj+9Uxw4s z5D{^^WY+ACLJxpXP(H?Pv3X zMqzq*U5PAjd?WL z6JjXDqDHV$Pt*ZyR+VG!L)ltm`0@m8s%+j%(!h=5c8^cQH}n$9gHnnu;Ag-o8UVGTPohpL-UgTOb*H^K@7@8VyiVBfv?C#QFiSSjkz$Ls=n5?{M%U>rT4xQq!s^sz z6$1fhHSHD?-v@6;M@IeKLep+-{O}*ZB9+0@kj5IB!=21UIxBED^LQaE!j`D^3Rs#+ zV6wzLBm}~Xu&$CxPsZ*5+ejR+G$bMR3o9#U6{NEY@H$~tG3H4M(j+pp_E1P&0RIB7 z@X9O3VflB+%nmCL0^MY*1`WZ9OlBTZ`*)*|a8Tv2(m;TE${g<`=pw6_FHLdN7-#x*tWGh{g zt)?FQbpC8DgDGf_#!1&+)5)Jxlg96p*Yw#RaIPBnj%~z-udwf&T-4{xf_+Nz2Cw2oWnR)H!5RAj$ySh5y<_&8i68!7%ujHQk88`j#)a3sen*!tujO97vRsv|H*{!R z;WRf&t0j4DKV34(=27Jg<1y<}+~4YJ`a(P*SMs!lYOBAXgR+L#Ft1}M)-lajzCp2% zoeK(HtBQ5St980)yI2Y80$NS30J{nczbS7X)qz>$b-wro%ihAhG|-^`QFDHGlv>dK zl5$PHrWh-vQdh%Er;p|XFd~;odvUj&hxB=psZn<4F{ubU$1}`Dj5YLGG4{{9GvIG` zyKlex`%k~T^{>08?h*yW98p9!W0;j~c^o#?l`gCH&vm7C&ULqj?h{?BC&T_pBvb+2 zbL6C%;Pbni%O-M*8W>HGQ;rDZZX|SdXsQoYNM4Mi2=$qX;H)!J-p@kO3!nF+p!i}O z^7es_@AyMPRNrfGy4E_|K@g@T3zsE$TC|kG(c4 z%8AE=(KOG~ZW5uM6{?nWdZX#K{;$>*C`N?-cc2!puwpdCgGRyyK5$H-D}DsnScb6| kBYy6*CgtYY_73XbaXV-l7+v_&^<6Lb;|1Ag&qVpCkGuS&zVAThQ6SHq@ zA$sz8-are6qGWaL3y6Q$aETbi1lM4RaZ&a|Mc4uI6B6SNQZ%@-pTg@AJYU%n;1HgeoxF|w}P@vE8c@V}7H?YPlJjvv!jMODbA z9V3yoJ=C4Qd2yj~&D}IFP21#ijlU|e`zT=0Blyto$V<9QkH`_dL-y$&*&{co*+cs% w^O>48x-)(JntW}GN_U5s=eg!8y2d)Mh;I?#`;VGd%xd{VI1wB24#`*aZ@ha~hX4Qo diff --git a/mpc_demo/__pycache__/utils.cpython-36.pyc b/mpc_demo/__pycache__/utils.cpython-36.pyc deleted file mode 100644 index 55f287c22f54e2276f878d394c4c5bbe4e02d247..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1217 zcmah}&u-H&7`L6I&9W8&LK|p+EX09I(<(sXfDjZGD+{EP3I9`CtK9r$%+B?r^e- zUXu$=E!ii(a`^AuD%9-bc^7KB+~e4Wb+-=Iny5Dq?b0F+INnKYj=BHNn=k!Qb7GC7 zng~#$+N+o95OB%7VI0+d9UZ;d1yv;+2^D)nS2WFL zkms4POeTUFD-?ZJY;8m>?rp_1DZ;&a+*yPIG9YWqO#+X%;SO4x8(3|EZv+MNmIxY74i#d>{&xM@|Apu4jkxXp1C+L(_?S794X<;W? znv*+eHZf*O&}%o>HwP{A1CSOS5SVo1!9DhOGsy>i7qs#^FBj4gshnpfh4alzZC#+s zsnE{nS(>^q&!kZ-5zYh3x`A>Xrj(G}b(u~;@EfkP2ihjPgHxk$A@ahj6rjR!jKUNY z%|ZT!2bqA2+%iDsv(Yl_wK?|~7>#;|^2CQENjh^RtsLR-U}~shL>?ETbGNX9D<&?*-(``cXY2}#q2mvby>ty1s@D# zF2aN&AH$EV;74SIT*X((X?%{H#b 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)