Skip to content

Commit

Permalink
Start working on sweep_local_tip_force_to_bending_torque_mapping
Browse files Browse the repository at this point in the history
  • Loading branch information
mstoelzle committed Nov 21, 2024
1 parent 839bf50 commit 5eef91d
Showing 1 changed file with 41 additions and 3 deletions.
44 changes: 41 additions & 3 deletions examples/simulate_pneumatic_planar_pcs.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,))
Expand All @@ -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()
Expand Down Expand Up @@ -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()

0 comments on commit 5eef91d

Please sign in to comment.