diff --git a/mpc_pybullet_demo/mpc_demo_nosim.py b/mpc_pybullet_demo/mpc_demo_nosim.py index 0f45edd..a870798 100755 --- a/mpc_pybullet_demo/mpc_demo_nosim.py +++ b/mpc_pybullet_demo/mpc_demo_nosim.py @@ -4,8 +4,8 @@ import numpy as np import matplotlib.pyplot as plt from matplotlib import animation -from utils import compute_path_from_wp -from cvxpy_mpc import optimize +from mpcpy.utils import compute_path_from_wp +import mpcpy import sys import time @@ -17,23 +17,32 @@ SIM_START_V=0. SIM_START_H=0. L=0.3 -from mpc_config import Params -P=Params() +P=mpcpy.Params() # Classes -class MPC(): +class MPCSim(): def __init__(self): # 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)) - self.opt_u[0,:] = 0.5 #m/ss - self.opt_u[1,:] = np.radians(0) #rad/s + #starting guess + self.action = np.zeros(P.M) + 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 - #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], [0,0,2,4,3,3,-1,-2,-6,-2,-2],P.path_tick) @@ -79,40 +88,48 @@ class MPC(): self.plot_sim() input("Press Enter to continue...") 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: - print("Success! Goal Reached") - input("Press Enter to continue...") - return + 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") + input("Press Enter to continue...") + return - #optimization loop - start=time.time() - self.opt_u = optimize(self.state, - self.opt_u, - self.path) + #optimization loop + #start=time.time() + + # State Matrices + A,B,C = mpcpy.get_linear_model_matrices(self.state, self.action) + + #TODO: check why taget does not update? + + #Get Reference_traj -> inputs are in worldframe + target, _ = mpcpy.get_ref_trajectory(self.state, + self.path, 1.0) - # print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start)) + 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.update_sim(self.opt_u[0,1],self.opt_u[1,1]) - self.predict_motion() - self.plot_sim() + 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): ''' - 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[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[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[2] = self.state[2] + acc*P.dt self.state[3] = self.state[3] + self.state[2]*np.tan(steer)/L*P.dt def plot_sim(self): - + ''' + ''' self.sim_time = self.sim_time+P.dt self.x_history.append(self.state[0]) self.y_history.append(self.state[1]) @@ -208,7 +225,7 @@ def plot_car(x, y, yaw): def do_sim(): - sim=MPC() + sim = MPCSim() try: sim.run() except Exception as e: diff --git a/mpc_pybullet_demo/mpc_demo_pybullet.py b/mpc_pybullet_demo/mpc_demo_pybullet.py index 79331bb..94623d1 100644 --- a/mpc_pybullet_demo/mpc_demo_pybullet.py +++ b/mpc_pybullet_demo/mpc_demo_pybullet.py @@ -2,11 +2,10 @@ import numpy as np import matplotlib.pyplot as plt from matplotlib import animation -from utils import compute_path_from_wp -from cvxpy_mpc import * +from mpcpy.utils import compute_path_from_wp +import mpcpy -from mpc_config import Params -P=Params() +P = mpcpy.Params() import sys import time @@ -130,7 +129,7 @@ def run_sim(): R = np.diag([10,10]) #input 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=[] y_history=[] @@ -169,10 +168,10 @@ def run_sim(): start=time.time() # 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 - target, _ = get_ref_trajectory(get_state(car), + target, _ = mpcpy.get_ref_trajectory(get_state(car), path, 1.0) x_mpc, u_mpc = mpc.optimize_linearized_model(A, B, C, state, target, time_horizon=P.T, verbose=False) diff --git a/mpc_pybullet_demo/mpcpy/__init__.py b/mpc_pybullet_demo/mpcpy/__init__.py new file mode 100644 index 0000000..ac2dcc8 --- /dev/null +++ b/mpc_pybullet_demo/mpcpy/__init__.py @@ -0,0 +1,2 @@ +from .cvxpy_mpc import * +from .mpc_config import Params diff --git a/mpc_pybullet_demo/cvxpy_mpc.py b/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py similarity index 98% rename from mpc_pybullet_demo/cvxpy_mpc.py rename to mpc_pybullet_demo/mpcpy/cvxpy_mpc.py index 7e1b155..64e3201 100755 --- a/mpc_pybullet_demo/cvxpy_mpc.py +++ b/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py @@ -5,9 +5,9 @@ from scipy.integrate import odeint from scipy.interpolate import interp1d import cvxpy as opt -from utils import * +from .utils import * -from mpc_config import Params +from .mpc_config import Params P=Params() def get_linear_model_matrices(x_bar,u_bar): diff --git a/mpc_pybullet_demo/mpc_config.py b/mpc_pybullet_demo/mpcpy/mpc_config.py similarity index 100% rename from mpc_pybullet_demo/mpc_config.py rename to mpc_pybullet_demo/mpcpy/mpc_config.py diff --git a/mpc_pybullet_demo/utils.py b/mpc_pybullet_demo/mpcpy/utils.py similarity index 99% rename from mpc_pybullet_demo/utils.py rename to mpc_pybullet_demo/mpcpy/utils.py index f7d2f5b..01a9153 100755 --- a/mpc_pybullet_demo/utils.py +++ b/mpc_pybullet_demo/mpcpy/utils.py @@ -1,7 +1,7 @@ import numpy as np from scipy.interpolate import interp1d -from mpc_config import Params +from .mpc_config import Params P=Params() def compute_path_from_wp(start_xp, start_yp, step = 0.1): diff --git a/notebooks/3.0-MPC-v3-track-constrains.ipynb b/notebooks/3.0-MPC-v3-track-constrains.ipynb index 02da958..eb9f922 100644 --- a/notebooks/3.0-MPC-v3-track-constrains.ipynb +++ b/notebooks/3.0-MPC-v3-track-constrains.ipynb @@ -1088,9 +1088,9 @@ ], "metadata": { "kernelspec": { - "display_name": "Python [conda env:.conda-jupyter] *", + "display_name": "Python 3", "language": "python", - "name": "conda-env-.conda-jupyter-py" + "name": "python3" }, "language_info": { "codemirror_mode": { @@ -1102,7 +1102,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.8.5" + "version": "3.8.3" } }, "nbformat": 4,