8 from proxsuite_nlp.utils
import plot_pd_errs
10 prim_infeas = cb.storage.prim_infeas.tolist()
11 dual_infeas = cb.storage.dual_infeas.tolist()
13 prim_infeas.append(res.primal_infeas)
14 dual_infeas.append(res.dual_infeas)
15 plot_pd_errs(ax, prim_infeas, dual_infeas)
16 ax.grid(axis=
"y", which=
"major")
20def plot_se2_pose(q: np.ndarray, ax: plt.Axes, alpha=0.5, fc=
"tab:blue"):
21 from matplotlib
import transforms
25 center = (q[0] - 0.5 * w, q[1] - 0.5 * h)
26 rect = plt.Rectangle(center, w, h, fc=fc, alpha=alpha)
27 theta = np.arctan2(q[3], q[2])
28 transform_ = transforms.Affine2D().rotate_around(*q[:2], -theta) + ax.transData
29 rect.set_transform(transform_)
56 nrows, r = divmod(nu, ncols)
59 fig, axes = plt.subplots(nrows, ncols, sharex=
"col", figsize=figsize)
61 fig = axes.flat[0].get_figure()
64 if rmodel
is not None:
65 effort_limit = rmodel.effortLimit
66 joint_names = rmodel.names
69 ax: plt.Axes = axes[i]
70 ax.step(times[:-1], us[:, i])
71 if effort_limit
is not None:
73 ax.hlines(-effort_limit[i], t0, tf, colors=
"k", linestyles=
"--")
74 ax.hlines(+effort_limit[i], t0, tf, colors=
"r", linestyles=
"dashdot")
76 if joint_names
is not None:
77 joint_name = joint_names[i].lower()
78 ax.set_ylabel(joint_name)
79 fig.supxlabel(
"Time $t$")
80 fig.suptitle(
"Control trajectories")
86 times, vs, rmodel, axes=None, ncols=2, figsize=(6.4, 6.4)
90 idx_to_joint_id_map = {}
93 if i
in rmodel.idx_vs.tolist():
95 idx_to_joint_id_map[i] = jid
96 nrows, r = divmod(nv, ncols)
99 vel_limit = rmodel.velocityLimit
104 fig, axes = plt.subplots(nrows, ncols, figsize=figsize)
107 fig = axes.flat[0].get_figure()
111 ax: plt.Axes = axes[i]
112 ax.plot(times, vs[:, i])
113 jid = idx_to_joint_id_map[i]
114 joint_name = rmodel.names[jid].lower()
116 ax.hlines(-vel_limit[i], t0, tf, colors=
"k", linestyles=
"--")
117 ax.hlines(+vel_limit[i], t0, tf, colors=
"r", linestyles=
"dashdot")
119 ax.set_ylabel(joint_name)
121 fig.supxlabel(
"Time $t$")
122 fig.suptitle(
"Velocity trajectories")
plt.Figure plot_controls_traj(times, us, ncols=2, axes=None, effort_limit=None, joint_names=None, rmodel=None, figsize=(6.4, 6.4))