From 5eef91d9c9494cade1ffa42de529e0a7d676742b Mon Sep 17 00:00:00 2001
From: Maximilian Stolzle <maximilian@stoelzle.ch>
Date: Thu, 21 Nov 2024 14:50:05 -0500
Subject: [PATCH] Start working on
 `sweep_local_tip_force_to_bending_torque_mapping`

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

diff --git a/examples/simulate_pneumatic_planar_pcs.py b/examples/simulate_pneumatic_planar_pcs.py
index 98f1b9c..01a20b7 100644
--- a/examples/simulate_pneumatic_planar_pcs.py
+++ b/examples/simulate_pneumatic_planar_pcs.py
@@ -58,6 +58,41 @@
     auxiliary_fns["jacobian_fn"],
 )
 
+def sweep_local_tip_force_to_bending_torque_mapping():
+    def compute_bending_torque(q: Array) -> Array:
+        # backbone coordinate of the end-effector
+        s_ee = jnp.sum(params["l"])
+        # compute the pose of the end-effector
+        chi_ee = forward_kinematics_fn(params, q, s_ee)
+        # orientation of the end-effector
+        th_ee = chi_ee[2]
+        # compute the jacobian of the end-effector
+        J_ee = auxiliary_fns["jacobian_fn"](params, q, s_ee)
+        # local tip force
+        f_ee_local = jnp.array([0.0, 1.0])
+        # tip force in inertial frame
+        f_ee = jnp.array([[jnp.cos(th_ee), -jnp.sin(th_ee)], [jnp.sin(th_ee), jnp.cos(th_ee)]]) @ f_ee_local
+        # compute the generalized torque
+        tau_be = J_ee[:2, 0].T @ f_ee
+        return tau_be
+
+    kappa_be_pts = jnp.arange(-2*jnp.pi, 2*jnp.pi, 0.01)
+    sigma_ax_pts = jnp.zeros_like(kappa_be_pts)
+    q_pts = jnp.stack([kappa_be_pts, sigma_ax_pts], axis=-1)
+
+    tau_be_pts = vmap(compute_bending_torque)(q_pts)
+
+    # plot the mapping on the bending strain
+    fig, ax = plt.subplots(num="planar_pcs_local_tip_force_to_bending_torque_mapping")
+    plt.title(r"Mapping from $f_\mathrm{ee}$ to $\tau_\mathrm{be}$")
+    ax.plot(kappa_be_pts, tau_be_pts, linewidth=2.5)
+    ax.set_xlabel(r"$\kappa_\mathrm{be}$ [rad/m]")
+    ax.set_ylabel(r"$\tau_\mathrm{be}$ [N m]")
+    plt.grid(True)
+    plt.tight_layout()
+    plt.show()
+
+
 def sweep_actuation_mapping():
     # evaluate the actuation matrix for a straight backbone
     q = jnp.zeros((2 * num_segments,))
@@ -70,12 +105,14 @@ def sweep_actuation_mapping():
     A_pts = vmap(actuation_mapping_fn, in_axes=(None, None, 0))(params, B_xi, q_pts)
     # 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_1$ to $\tau_\mathrm{be}$")
-    ax.plot(kappa_be_pts, A_pts[:, 0, 0], linewidth=2)
+    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].min(), 0.0, facecolor='red', alpha=0.5)
+    ax.axhspan(A_pts[:, 0, 0:2].min(), 0.0, 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]")
     ax.set_ylabel(r"$\frac{\partial \tau_\mathrm{be}}{\partial u_1}$")
+    plt.legend()
     plt.grid(True)
     plt.tight_layout()
     plt.show()
@@ -225,5 +262,6 @@ def simulate_robot():
     plt.show()
 
 if __name__ == "__main__":
+    sweep_local_tip_force_to_bending_torque_mapping()
     sweep_actuation_mapping()
     simulate_robot()