From 362493e4b33db51e154b235742d627d649e4a5ae Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= <maximilian@stoelzle.ch>
Date: Thu, 21 Nov 2024 16:02:06 -0500
Subject: [PATCH] Update `sweep_actuation_mapping`

---
 examples/simulate_pneumatic_planar_pcs.py | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/examples/simulate_pneumatic_planar_pcs.py b/examples/simulate_pneumatic_planar_pcs.py
index 01a20b7..a26b876 100644
--- a/examples/simulate_pneumatic_planar_pcs.py
+++ b/examples/simulate_pneumatic_planar_pcs.py
@@ -99,15 +99,21 @@ def sweep_actuation_mapping():
     A = actuation_mapping_fn(params, B_xi, q)
     print("Evaluating actuation matrix for straight backbone: A =\n", A)
 
-    kappa_be_pts = jnp.linspace(-jnp.pi, jnp.pi, 500)
+    kappa_be_pts = jnp.linspace(-3*jnp.pi, 3*jnp.pi, 500)
     sigma_ax_pts = jnp.zeros_like(kappa_be_pts)
     q_pts = jnp.stack([kappa_be_pts, sigma_ax_pts], axis=-1)
     A_pts = vmap(actuation_mapping_fn, in_axes=(None, None, 0))(params, B_xi, q_pts)
+    # mark the points that are not controllable as the u1 and u2 terms share the same sign
+    non_controllable_selector = A_pts[..., 0, 0] * A_pts[..., 0, 1] >= 0.0
+    non_controllable_indices = jnp.where(non_controllable_selector)[0]
+    non_controllable_boundary_indices = jnp.where(non_controllable_selector[:-1] != non_controllable_selector[1:])[0]
     # plot the mapping on the bending strain for various bending strains
     fig, ax = plt.subplots(num="pneumatic_planar_pcs_actuation_mapping_bending_torque_vs_bending_strain")
     plt.title(r"Actuation mapping from $u$ to $\tau_\mathrm{be}$")
-    # shade the region where the actuation mapping is negative as we are not able to bend the robot further
-    ax.axhspan(A_pts[:, 0, 0:2].min(), 0.0, facecolor='red', alpha=0.2)
+    # # shade the region where the actuation mapping is negative as we are not able to bend the robot further
+    # ax.axhspan(A_pts[:, 0, 0:2].min(), 0.0, facecolor='red', alpha=0.2)
+    for idx in non_controllable_indices:
+        ax.axvspan(kappa_be_pts[idx], kappa_be_pts[idx+1], facecolor='red', alpha=0.2)
     ax.plot(kappa_be_pts, A_pts[:, 0, 0], linewidth=2, label=r"$\frac{\partial \tau_\mathrm{be}}{\partial u_1}$")
     ax.plot(kappa_be_pts, A_pts[:, 0, 1], linewidth=2, label=r"$\frac{\partial \tau_\mathrm{ax}}{\partial u_2}$")
     ax.set_xlabel(r"$\kappa_\mathrm{be}$ [rad/m]")