tidy up a bit python files location

master
mcarfagno 2021-07-08 12:54:48 +01:00
parent 50f1fea9a8
commit 4ffbb35207
7 changed files with 65 additions and 47 deletions

View File

@ -4,8 +4,8 @@ import numpy as np
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from matplotlib import animation from matplotlib import animation
from utils import compute_path_from_wp from mpcpy.utils import compute_path_from_wp
from cvxpy_mpc import optimize import mpcpy
import sys import sys
import time import time
@ -17,23 +17,32 @@ SIM_START_V=0.
SIM_START_H=0. SIM_START_H=0.
L=0.3 L=0.3
from mpc_config import Params P=mpcpy.Params()
P=Params()
# Classes # Classes
class MPC(): class MPCSim():
def __init__(self): def __init__(self):
# State for the robot mathematical model [x,y,heading] # State for the robot mathematical model [x,y,heading]
self.state = [SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H] self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H])
self.opt_u = np.zeros((P.M,P.T)) #starting guess
self.opt_u[0,:] = 0.5 #m/ss self.action = np.zeros(P.M)
self.opt_u[1,:] = np.radians(0) #rad/s self.action[0] = P.MAX_ACC/2 #a
self.action[1] = 0.0 #delta
self.opt_u = np.zeros((P.M,P.T))
# Cost Matrices
Q = np.diag([20,20,10,20]) #state error cost
Qf = np.diag([30,30,30,30]) #state final error cost
R = np.diag([10,10]) #input cost
R_ = np.diag([10,10]) #input rate of change cost
self.mpc = mpcpy.MPC(P.N,P.M,Q,R)
# Interpolated Path to follow given waypoints # Interpolated Path to follow given waypoints
#self.path = compute_path_from_wp([0,10,12,2,4,14],[0,0,2,10,12,12])
self.path = compute_path_from_wp([0,3,4,6,10,12,13,13,6,1,0], self.path = compute_path_from_wp([0,3,4,6,10,12,13,13,6,1,0],
[0,0,2,4,3,3,-1,-2,-6,-2,-2],P.path_tick) [0,0,2,4,3,3,-1,-2,-6,-2,-2],P.path_tick)
@ -79,40 +88,48 @@ class MPC():
self.plot_sim() self.plot_sim()
input("Press Enter to continue...") input("Press Enter to continue...")
while 1: while 1:
if self.state is not None:
if np.sqrt((self.state[0]-self.path[0,-1])**2+(self.state[1]-self.path[1,-1])**2)<0.1: if np.sqrt((self.state[0]-self.path[0,-1])**2+(self.state[1]-self.path[1,-1])**2)<0.1:
print("Success! Goal Reached") print("Success! Goal Reached")
input("Press Enter to continue...") input("Press Enter to continue...")
return return
#optimization loop #optimization loop
start=time.time() #start=time.time()
self.opt_u = optimize(self.state,
self.opt_u,
self.path)
# print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start)) # State Matrices
A,B,C = mpcpy.get_linear_model_matrices(self.state, self.action)
self.update_sim(self.opt_u[0,1],self.opt_u[1,1]) #TODO: check why taget does not update?
self.predict_motion()
self.plot_sim() #Get Reference_traj -> inputs are in worldframe
target, _ = mpcpy.get_ref_trajectory(self.state,
self.path, 1.0)
x_mpc, u_mpc = self.mpc.optimize_linearized_model(A, B, C, self.state, target, time_horizon=P.T, verbose=False)
self.opt_u = np.vstack((np.array(u_mpc.value[0,:]).flatten(),
(np.array(u_mpc.value[1,:]).flatten())))
self.action[:] = [u_mpc.value[0,1],u_mpc.value[1,1]]
# print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
self.update_sim(self.action[0],self.action[1])
self.predict_motion()
self.plot_sim()
def update_sim(self,acc,steer): def update_sim(self,acc,steer):
''' '''
Updates state.
:param lin_v: float
:param ang_v: float
''' '''
self.state[0] = self.state[0] + self.state[2]*np.cos(self.state[3])*P.dt
self.state[0] = self.state[0] +self.state[2]*np.cos(self.state[3])*P.dt self.state[1] = self.state[1] + self.state[2]*np.sin(self.state[3])*P.dt
self.state[1] = self.state[1] +self.state[2]*np.sin(self.state[3])*P.dt self.state[2] = self.state[2] + acc*P.dt
self.state[2] = self.state[2] +acc*P.dt
self.state[3] = self.state[3] + self.state[2]*np.tan(steer)/L*P.dt self.state[3] = self.state[3] + self.state[2]*np.tan(steer)/L*P.dt
def plot_sim(self): def plot_sim(self):
'''
'''
self.sim_time = self.sim_time+P.dt self.sim_time = self.sim_time+P.dt
self.x_history.append(self.state[0]) self.x_history.append(self.state[0])
self.y_history.append(self.state[1]) self.y_history.append(self.state[1])
@ -208,7 +225,7 @@ def plot_car(x, y, yaw):
def do_sim(): def do_sim():
sim=MPC() sim = MPCSim()
try: try:
sim.run() sim.run()
except Exception as e: except Exception as e:

View File

@ -2,11 +2,10 @@ import numpy as np
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from matplotlib import animation from matplotlib import animation
from utils import compute_path_from_wp from mpcpy.utils import compute_path_from_wp
from cvxpy_mpc import * import mpcpy
from mpc_config import Params P = mpcpy.Params()
P=Params()
import sys import sys
import time import time
@ -130,7 +129,7 @@ def run_sim():
R = np.diag([10,10]) #input cost R = np.diag([10,10]) #input cost
R_ = np.diag([10,10]) #input rate of change cost R_ = np.diag([10,10]) #input rate of change cost
mpc = MPC(P.N,P.M,Q,R) mpc = mpcpy.MPC(P.N,P.M,Q,R)
x_history=[] x_history=[]
y_history=[] y_history=[]
@ -169,10 +168,10 @@ def run_sim():
start=time.time() start=time.time()
# State Matrices # State Matrices
A,B,C = get_linear_model_matrices(state, action) A,B,C = mpcpy.get_linear_model_matrices(state, action)
#Get Reference_traj -> inputs are in worldframe #Get Reference_traj -> inputs are in worldframe
target, _ = get_ref_trajectory(get_state(car), target, _ = mpcpy.get_ref_trajectory(get_state(car),
path, 1.0) path, 1.0)
x_mpc, u_mpc = mpc.optimize_linearized_model(A, B, C, state, target, time_horizon=P.T, verbose=False) x_mpc, u_mpc = mpc.optimize_linearized_model(A, B, C, state, target, time_horizon=P.T, verbose=False)

View File

@ -0,0 +1,2 @@
from .cvxpy_mpc import *
from .mpc_config import Params

View File

@ -5,9 +5,9 @@ from scipy.integrate import odeint
from scipy.interpolate import interp1d from scipy.interpolate import interp1d
import cvxpy as opt import cvxpy as opt
from utils import * from .utils import *
from mpc_config import Params from .mpc_config import Params
P=Params() P=Params()
def get_linear_model_matrices(x_bar,u_bar): def get_linear_model_matrices(x_bar,u_bar):

View File

@ -1,7 +1,7 @@
import numpy as np import numpy as np
from scipy.interpolate import interp1d from scipy.interpolate import interp1d
from mpc_config import Params from .mpc_config import Params
P=Params() P=Params()
def compute_path_from_wp(start_xp, start_yp, step = 0.1): def compute_path_from_wp(start_xp, start_yp, step = 0.1):

View File

@ -1088,9 +1088,9 @@
], ],
"metadata": { "metadata": {
"kernelspec": { "kernelspec": {
"display_name": "Python [conda env:.conda-jupyter] *", "display_name": "Python 3",
"language": "python", "language": "python",
"name": "conda-env-.conda-jupyter-py" "name": "python3"
}, },
"language_info": { "language_info": {
"codemirror_mode": { "codemirror_mode": {
@ -1102,7 +1102,7 @@
"name": "python", "name": "python",
"nbconvert_exporter": "python", "nbconvert_exporter": "python",
"pygments_lexer": "ipython3", "pygments_lexer": "ipython3",
"version": "3.8.5" "version": "3.8.3"
} }
}, },
"nbformat": 4, "nbformat": 4,