updated README with nicer gifs from latest implementation

README.md

@@ -36,6 +36,8 @@ The MPC implementation is tested using **[bullet](https://pybullet.org/wordpress
 
 Results:
 
+![](img/demo_bullet.gif)
+
 ![](img/demo.gif)
 
 To run the pybullet demo:
@@ -59,4 +61,4 @@ pip3 install --user --requirement requirements.txt
 ## References
 * [mpc paper](https://borrelli.me.berkeley.edu/pdfpub/IV_KinematicMPC_jason.pdf)
 * [pythonrobotics](https://github.com/AtsushiSakai/PythonRobotics/)
-* [pybullet](https://pybullet.org/wordpress/)
+* [pybullet](https://pybullet.org/wordpress/)

mpc_pybullet_demo/mpc_demo_nosim.py

@@ -76,12 +76,14 @@ class MPC():
     def run(self):
         '''
         '''
-
+        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.5:
+                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
@@ -137,7 +139,7 @@ class MPC():
 
         if self.predicted is not None:
             plt.plot(self.predicted[0,:], self.predicted[1,:], c='tab:green',
-                                                     marker=".",
+                                                     marker="+",
                                                      alpha=0.5,
                                                      label="mpc opt trajectory")
 
@@ -186,8 +188,8 @@ class MPC():
 
 
 def plot_car(x, y, yaw):
-    LENGTH = 0.35  # [m]
-    WIDTH = 0.2  # [m]
+    LENGTH = 0.5  # [m]
+    WIDTH = 0.25  # [m]
     OFFSET = LENGTH  # [m]
 
     outline = np.array([[-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET],

mpc_pybullet_demo/mpc_demo_pybullet.py

@@ -130,10 +130,11 @@ def run_sim():
         #track path in bullet
         p.addUserDebugLine([state[0],state[1],0],[state[0],state[1],0.5],[1,0,0])
 
-        if np.sqrt((state[0]-path[0,-1])**2+(state[1]-path[1,-1])**2)<1:
+        if np.sqrt((state[0]-path[0,-1])**2+(state[1]-path[1,-1])**2)<0.1:
             print("Success! Goal Reached")
             set_ctrl(car,0,0,0)
             plot_results(path,x_history,y_history)
+            input("Press Enter to continue...")
             p.disconnect()
             return