updated README with nicer gifs from latest implementation

master
mcarfagno 2020-11-02 14:44:31 +00:00
parent 271b220277
commit 3546934f75
6 changed files with 12 additions and 7 deletions

View File

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

Binary file not shown.

Before

Width:  |  Height:  |  Size: 3.3 MiB

After

Width:  |  Height:  |  Size: 4.0 MiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 1.6 MiB

BIN
img/demo_bullet.gif Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 284 KiB

View File

@ -76,12 +76,14 @@ class MPC():
def run(self): def run(self):
''' '''
''' '''
self.plot_sim()
input("Press Enter to continue...")
while 1: while 1:
if self.state is not None: 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") print("Success! Goal Reached")
input("Press Enter to continue...")
return return
#optimization loop #optimization loop
@ -137,7 +139,7 @@ class MPC():
if self.predicted is not None: if self.predicted is not None:
plt.plot(self.predicted[0,:], self.predicted[1,:], c='tab:green', plt.plot(self.predicted[0,:], self.predicted[1,:], c='tab:green',
marker=".", marker="+",
alpha=0.5, alpha=0.5,
label="mpc opt trajectory") label="mpc opt trajectory")
@ -186,8 +188,8 @@ class MPC():
def plot_car(x, y, yaw): def plot_car(x, y, yaw):
LENGTH = 0.35 # [m] LENGTH = 0.5 # [m]
WIDTH = 0.2 # [m] WIDTH = 0.25 # [m]
OFFSET = LENGTH # [m] OFFSET = LENGTH # [m]
outline = np.array([[-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET], outline = np.array([[-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET],

View File

@ -130,10 +130,11 @@ def run_sim():
#track path in bullet #track path in bullet
p.addUserDebugLine([state[0],state[1],0],[state[0],state[1],0.5],[1,0,0]) 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") print("Success! Goal Reached")
set_ctrl(car,0,0,0) set_ctrl(car,0,0,0)
plot_results(path,x_history,y_history) plot_results(path,x_history,y_history)
input("Press Enter to continue...")
p.disconnect() p.disconnect()
return return