tidy up a bit python files location

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 mpcpy.utils import compute_path_from_wp
-from cvxpy_mpc import optimize
+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=mpcpy.Params()
-P=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))
+        #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
-        #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:
+            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):
         '''
-        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
 
     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:

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 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 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)

mpc_pybullet_demo/mpcpy/__init__.py

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

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):

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):

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,