Thanks to visit codestin.com
Credit goes to github.com

Skip to content

minor changes of zoRO examples #946

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 10 commits into from
Jul 18, 2023
Merged

minor changes of zoRO examples #946

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
3caf7a0
conditional import latex
Jul 14, 2023
a169047
better plotting for diff-drive robot trajectory
Jul 14, 2023
85c9ee0
debug use
Aug 26, 2022
80f5dfd
Cleanup Python getting_started folder (#881)
FreyJo Nov 15, 2022
67e6129
minor change of labels in the plots
yf-gao Jul 17, 2023
8b5ef28
conditonal use latex
yf-gao Jul 17, 2023
23b3926
Revert "debug use"
yf-gao Jul 17, 2023
93c8a39
clean merge artifacts
yf-gao Jul 17, 2023
f146ed6
example_sqp_rti_loop.py: remove additional line at the end
yf-gao Jul 17, 2023
fb92f81
update diff_drive_utils.py
yf-gao Jul 17, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 39 additions & 7 deletions examples/acados_python/zoRO_example/diff_drive/diff_drive_utils.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,19 +30,21 @@
import matplotlib.pyplot as plt
import matplotlib
import os
import shutil

from mpc_parameters import MPCParam


def get_latex_plot_params():
text_usetex = True if shutil.which('latex') else False
params = {'backend': 'ps',
'text.latex.preamble': r"\usepackage{gensymb} \usepackage{amsmath}",
'axes.labelsize': 12,
'axes.titlesize': 12,
'legend.fontsize': 12,
'xtick.labelsize': 12,
'ytick.labelsize': 12,
'text.usetex': True,
'text.usetex': text_usetex,
'font.family': 'serif'
}

Expand All @@ -63,20 +65,50 @@ def plot_timings(timing_dict):
fig = plt.figure(0)
ax = fig.add_subplot(111)
ax.boxplot(timing_dict.values(), vert=False,
flierprops=green_square, \
medianprops=medianprops, showmeans=False)
flierprops=green_square,
medianprops=medianprops, showmeans=False
)
ax.set_yticklabels(timing_dict.keys())
plt.grid()
plt.xlabel('CPU time [ms]')
plt.xlabel("CPU time [ms]")
plt.tight_layout()

if not os.path.exists('figures'):
os.makedirs('figures')
if not os.path.exists("figures"):
os.makedirs("figures")
plt.savefig(os.path.join("figures", "timings_diff_drive.pdf"),
bbox_inches='tight', transparent=True, pad_inches=0.05)


def compute_min_dis(cfg:MPCParam, s:np.ndarray)->float:
def plot_trajectory(cfg:MPCParam, traj_ref:np.ndarray, traj_zo:np.ndarray):
# latexify plot
params = get_latex_plot_params()
matplotlib.rcParams.update(params)

fig = plt.figure(1)
ax = fig.add_subplot(1,1,1)
for idx_obs in range(cfg.num_obs):
circ_label = "Obstacle" if idx_obs == 0 else None
circ = plt.Circle(cfg.obs_pos[idx_obs,:], cfg.obs_radius[idx_obs],
edgecolor="red", facecolor=(1,0,0,.5), label=circ_label
)
ax.add_artist(circ)
ax.set_title("Robot Trajectory")
ax.plot(traj_ref[:, 0], traj_ref[:, 1], c='m', label='Ref Traj')
ax.plot(traj_zo[:, 0], traj_zo[:, 1], c='b', label='zoRO Traj')
ax.set_xlabel("x [m]")
ax.set_ylabel("y [m]")
ax.set_xticks(np.arange(-2., 9., 2.))
ax.set_yticks(np.arange(0., 11., 2.))
ax.set_aspect("equal")
ax.legend()

if not os.path.exists("figures"):
os.makedirs("figures")
plt.savefig(os.path.join("figures", "diff_drive_sim_trajectory.pdf"),
bbox_inches='tight', transparent=True, pad_inches=0.05)


def compute_min_dis(cfg:MPCParam, s:np.ndarray) -> float:
min_dist = np.inf
for idx_obs in range(cfg.num_obs):
min_dist = np.min([min_dist, \
Expand Down
20 changes: 2 additions & 18 deletions examples/acados_python/zoRO_example/diff_drive/main.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@

from diff_drive_zoro_mpc import ZoroMPCSolver
from mpc_parameters import MPCParam, PathTrackingParam
from diff_drive_utils import plot_timings, compute_min_dis
from diff_drive_utils import plot_timings, plot_trajectory, compute_min_dis
from nominal_path_tracking_casadi import NominalPathTrackingSolver
from track_spline import TrackSpline

Expand Down Expand Up @@ -139,23 +139,7 @@ def main():
"total": 1e3*np.array(total_time)
}
plot_timings(timing_dict)

# plot trajectory
fig = plt.figure(1)
plt.rcParams['font.size'] = '16'
ax = fig.add_subplot(1,1,1)
for idx_obs in range(cfg_zo.num_obs):
circ = plt.Circle(cfg_zo.obs_pos[idx_obs,:], cfg_zo.obs_radius[idx_obs], \
edgecolor="red", facecolor=(1,0,0,.5))
ax.add_artist(circ)
plt.plot(path_tracking_solver.x_robot_ref[:, 0], path_tracking_solver.x_robot_ref[:, 1], c='m', label='ref')
plt.plot(traj_zo[:, 0], traj_zo[:, 1], c='b', label='opt sqp')
plt.legend()

if not os.path.exists('figures'):
os.makedirs('figures')
plt.savefig(os.path.join("figures", "diff_drive_sim_trajectory.pdf"),
bbox_inches='tight', transparent=True, pad_inches=0.05)
plot_trajectory(cfg_zo, path_tracking_solver.x_robot_ref, traj_zo)


if __name__ == "__main__":
Expand Down