Skip to content

Commit

Permalink
[examples] fix talos_arms.py
Browse files Browse the repository at this point in the history
  • Loading branch information
ManifoldFR committed Oct 22, 2024
1 parent be9e604 commit e9dd5a0
Showing 1 changed file with 8 additions and 6 deletions.
14 changes: 8 additions & 6 deletions examples/talos_arms.py
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,7 @@ class Args(ArgsBase):
if args.bounds:
umax = rmodel.effortLimit
umin = -umax
print(f"Effort limit: {umax}")
# fun: u -> u
ctrl_fn = aligator.ControlErrorResidual(space.ndx, np.zeros(nu))
stm.addConstraint(ctrl_fn, constraints.BoxConstraint(umin, umax))
Expand Down Expand Up @@ -118,12 +119,13 @@ class Args(ArgsBase):
times = np.linspace(0.0, Tf, nsteps + 1)
us_opt = np.array(results.us)
ls = plt.plot(times[1:], results.us)
mask_where_ctrl_saturate = np.any((us_opt <= umin) | (us_opt >= umax), axis=0)
idx_hit = np.argwhere(mask_where_ctrl_saturate).flatten()
if len(idx_hit) > 0:
ls[idx_hit[0]].set_label("u{}".format(idx_hit[0]))
plt.hlines(umin[idx_hit], *times[[0, -1]], colors="r", linestyles="--")
plt.hlines(umax[idx_hit], *times[[0, -1]], colors="b", linestyles="--")
if args.bounds:
mask_where_ctrl_saturate = np.any((us_opt <= umin) | (us_opt >= umax), axis=0)
idx_hit = np.argwhere(mask_where_ctrl_saturate).flatten()
if len(idx_hit) > 0:
ls[idx_hit[0]].set_label("u{}".format(idx_hit[0]))
plt.hlines(umin[idx_hit], *times[[0, -1]], colors="r", linestyles="--")
plt.hlines(umax[idx_hit], *times[[0, -1]], colors="b", linestyles="--")
plt.title("Controls trajectory")
plt.legend()
plt.tight_layout()
Expand Down

0 comments on commit e9dd5a0

Please sign in to comment.