From 4ec93027653528c03b0917a239f6c32d3f375218 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Fri, 22 Nov 2024 18:55:08 -0500 Subject: [PATCH] Add a planar pcs system with pneumatic actuation (#5) * Add `B_xi` to `stiffness_fn` interface * Start working on pneumatic planar pcs system * Start working on `actuation_mapping_fn` * Already bump version such as that we release a new version when merging into main * Fix bug in `energy_fn` * Allow setting custom `actuation_mapping_fn` for planar pcs systems * Properly implement `actuation_basis` * Continue implementing `actuation_mapping_fn` * Continue working on `compute_actuation_matrix_for_segment` * Fully implement pneumatic actuation model * Start working on `sweep_local_tip_force_to_bending_torque_mapping` * Update `sweep_actuation_mapping` * Fix small bug * Update src/jsrm/systems/pneumatic_planar_pcs.py Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --------- Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- examples/simulate_planar_pcs.py | 2 +- examples/simulate_pneumatic_planar_pcs.py | 273 ++++++++++++++++++++++ pyproject.toml | 2 +- src/jsrm/systems/planar_pcs.py | 43 +++- src/jsrm/systems/pneumatic_planar_pcs.py | 190 +++++++++++++++ src/jsrm/systems/utils.py | 2 +- 6 files changed, 503 insertions(+), 9 deletions(-) create mode 100644 examples/simulate_pneumatic_planar_pcs.py create mode 100644 src/jsrm/systems/pneumatic_planar_pcs.py diff --git a/examples/simulate_planar_pcs.py b/examples/simulate_planar_pcs.py index 53b6d00..92e962e 100644 --- a/examples/simulate_planar_pcs.py +++ b/examples/simulate_planar_pcs.py @@ -162,7 +162,7 @@ def draw_robot( ) plt.plot( video_ts, q_ts[:, 3 * segment_idx + 2], - label=r"$\sigma_\mathrm{el," + str(segment_idx + 1) + "}$ [-]" + label=r"$\sigma_\mathrm{ax," + str(segment_idx + 1) + "}$ [-]" ) plt.xlabel("Time [s]") plt.ylabel("Configuration") diff --git a/examples/simulate_pneumatic_planar_pcs.py b/examples/simulate_pneumatic_planar_pcs.py new file mode 100644 index 0000000..299f151 --- /dev/null +++ b/examples/simulate_pneumatic_planar_pcs.py @@ -0,0 +1,273 @@ +from functools import partial +import jax + +jax.config.update("jax_enable_x64", True) # double precision +from diffrax import diffeqsolve, Euler, ODETerm, SaveAt, Tsit5 +from jax import Array, vmap +from jax import numpy as jnp +import matplotlib.pyplot as plt +import numpy as onp +from pathlib import Path +from typing import Callable, Dict + +import jsrm +from jsrm import ode_factory +from jsrm.systems import pneumatic_planar_pcs + +num_segments = 1 + +# filepath to symbolic expressions +sym_exp_filepath = ( + Path(jsrm.__file__).parent + / "symbolic_expressions" + / f"planar_pcs_ns-{num_segments}.dill" +) + +# set parameters +rho = 1070 * jnp.ones((num_segments,)) # Volumetric density of Dragon Skin 20 [kg/m^3] +params = { + "th0": jnp.array(0.0), # initial orientation angle [rad] + "l": 1e-1 * jnp.ones((num_segments,)), + "r": 2e-2 * jnp.ones((num_segments,)), + "rho": rho, + "g": jnp.array([0.0, 9.81]), + "E": 2e3 * jnp.ones((num_segments,)), # Elastic modulus [Pa] + "G": 1e3 * jnp.ones((num_segments,)), # Shear modulus [Pa] + "r_cham_in": 5e-3 * jnp.ones((num_segments,)), + "r_cham_out": 2e-2 - 2e-3 * jnp.ones((num_segments,)), + "varphi_cham": jnp.pi/2 * jnp.ones((num_segments,)), +} +params["D"] = 5e-4 * jnp.diag( + (jnp.repeat( + jnp.array([[1e0, 1e3, 1e3]]), num_segments, axis=0 + ) * params["l"][:, None]).flatten() +) + +# activate all strains (i.e. bending, shear, and axial) +# strain_selector = jnp.ones((3 * num_segments,), dtype=bool) +strain_selector = jnp.array([True, False, True])[None, :].repeat(num_segments, axis=0).flatten() + +B_xi, forward_kinematics_fn, dynamical_matrices_fn, auxiliary_fns = ( + pneumatic_planar_pcs.factory(num_segments, sym_exp_filepath, strain_selector) +) +# jit the functions +dynamical_matrices_fn = jax.jit(dynamical_matrices_fn) +actuation_mapping_fn = partial( + auxiliary_fns["actuation_mapping_fn"], + forward_kinematics_fn, + 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,)) + A = actuation_mapping_fn(params, B_xi, q) + print("Evaluating actuation matrix for straight backbone: A =\n", A) + + 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) + 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]") + ax.set_ylabel(r"$\frac{\partial \tau_\mathrm{be}}{\partial u_1}$") + plt.legend() + plt.grid(True) + plt.tight_layout() + plt.show() + + # create grid for bending and axial strains + kappa_be_grid, sigma_ax_grid = jnp.meshgrid( + jnp.linspace(-jnp.pi, jnp.pi, 20), + jnp.linspace(-0.2, 0.2, 20), + ) + q_pts = jnp.stack([kappa_be_grid.flatten(), sigma_ax_grid.flatten()], axis=-1) + + # evaluate the actuation mapping on the grid + A_pts = vmap(actuation_mapping_fn, in_axes=(None, None, 0))(params, B_xi, q_pts) + # reshape A_pts to match the grid shape + A_grid = A_pts.reshape(kappa_be_grid.shape[:2] + A_pts.shape[-2:]) + + # plot the mapping on the bending strain + fig, ax = plt.subplots(num="pneumatic_planar_pcs_actuation_mapping_bending_torque_vs_axial_vs_bending_strain") + plt.title(r"Actuation mapping from $u_1$ to $\tau_\mathrm{be}$") + # contourf plot + c = ax.contourf(kappa_be_grid, sigma_ax_grid, A_grid[..., 0, 0], levels=100) + fig.colorbar(c, ax=ax, label=r"$\frac{\partial \tau_\mathrm{be}}{\partial u_1}$") + # contour plot + ax.contour(kappa_be_grid, sigma_ax_grid, A_grid[..., 0, 0], levels=20, colors="k", linewidths=0.5) + ax.set_xlabel(r"$\kappa_\mathrm{be}$ [rad/m]") + ax.set_ylabel(r"$\sigma_\mathrm{ax}$ [-]") + plt.tight_layout() + plt.show() + + # plot the mapping on the axial strain + fig, ax = plt.subplots(num="pneumatic_planar_pcs_actuation_mapping_axial_torque_vs_axial_vs_bending_strain") + plt.title(r"Actuation mapping from $u_1$ to $\tau_\mathrm{ax}$") + # contourf plot + c = ax.contourf(kappa_be_grid, sigma_ax_grid, A_grid[..., 1, 0], levels=100) + fig.colorbar(c, ax=ax, label=r"$\frac{\partial \tau_\mathrm{ax}}{\partial u_1}$") + # contour plot + ax.contour(kappa_be_grid, sigma_ax_grid, A_grid[..., 1, 0], levels=20, colors="k", linewidths=0.5) + ax.set_xlabel(r"$\kappa_\mathrm{be}$ [rad/m]") + ax.set_ylabel(r"$\sigma_\mathrm{ax}$ [-]") + plt.tight_layout() + plt.show() + + +def simulate_robot(): + # define initial configuration + q0 = jnp.repeat(jnp.array([-5.0 * jnp.pi, -0.2])[None, :], num_segments, axis=0).flatten() + # number of generalized coordinates + n_q = q0.shape[0] + + # set simulation parameters + dt = 1e-3 # time step + sim_dt = 5e-5 # simulation time step + ts = jnp.arange(0.0, 7.0, dt) # time steps + + x0 = jnp.concatenate([q0, jnp.zeros_like(q0)]) # initial condition + u = jnp.array([1.2e3, 0e0]) # control inputs (pressures in the right and left chambers) + + ode_fn = ode_factory(dynamical_matrices_fn, params, u) + term = ODETerm(ode_fn) + + sol = diffeqsolve( + term, + solver=Tsit5(), + t0=ts[0], + t1=ts[-1], + dt0=sim_dt, + y0=x0, + max_steps=None, + saveat=SaveAt(ts=ts), + ) + + print("sol.ys =\n", sol.ys) + # the evolution of the generalized coordinates + q_ts = sol.ys[:, :n_q] + # the evolution of the generalized velocities + q_d_ts = sol.ys[:, n_q:] + + # evaluate the forward kinematics along the trajectory + chi_ee_ts = vmap(forward_kinematics_fn, in_axes=(None, 0, None))( + params, q_ts, jnp.array([jnp.sum(params["l"])]) + ) + # plot the configuration vs time + plt.figure() + for segment_idx in range(num_segments): + plt.plot( + ts, q_ts[:, 2 * segment_idx + 0], + label=r"$\kappa_\mathrm{be," + str(segment_idx + 1) + "}$ [rad/m]" + ) + plt.plot( + ts, q_ts[:, 2 * segment_idx + 1], + label=r"$\sigma_\mathrm{ax," + str(segment_idx + 1) + "}$ [-]" + ) + plt.xlabel("Time [s]") + plt.ylabel("Configuration") + plt.legend() + plt.grid(True) + plt.tight_layout() + plt.show() + # plot end-effector position vs time + plt.figure() + plt.plot(ts, chi_ee_ts[:, 0], label="x") + plt.plot(ts, chi_ee_ts[:, 1], label="y") + plt.xlabel("Time [s]") + plt.ylabel("End-effector Position [m]") + plt.legend() + plt.grid(True) + plt.box(True) + plt.tight_layout() + plt.show() + # plot the end-effector position in the x-y plane as a scatter plot with the time as the color + plt.figure() + plt.scatter(chi_ee_ts[:, 0], chi_ee_ts[:, 1], c=ts, cmap="viridis") + plt.axis("equal") + plt.grid(True) + plt.xlabel("End-effector x [m]") + plt.ylabel("End-effector y [m]") + plt.colorbar(label="Time [s]") + plt.tight_layout() + plt.show() + # plt.figure() + # plt.plot(chi_ee_ts[:, 0], chi_ee_ts[:, 1]) + # plt.axis("equal") + # plt.grid(True) + # plt.xlabel("End-effector x [m]") + # plt.ylabel("End-effector y [m]") + # plt.tight_layout() + # plt.show() + + # plot the energy along the trajectory + kinetic_energy_fn_vmapped = vmap( + partial(auxiliary_fns["kinetic_energy_fn"], params) + ) + potential_energy_fn_vmapped = vmap( + partial(auxiliary_fns["potential_energy_fn"], params) + ) + U_ts = potential_energy_fn_vmapped(q_ts) + T_ts = kinetic_energy_fn_vmapped(q_ts, q_d_ts) + plt.figure() + plt.plot(ts, U_ts, label="Potential energy") + plt.plot(ts, T_ts, label="Kinetic energy") + plt.xlabel("Time [s]") + plt.ylabel("Energy [J]") + plt.legend() + plt.grid(True) + plt.box(True) + plt.tight_layout() + plt.show() + +if __name__ == "__main__": + sweep_local_tip_force_to_bending_torque_mapping() + sweep_actuation_mapping() + simulate_robot() diff --git a/pyproject.toml b/pyproject.toml index ec20a41..7c2a8b6 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -17,7 +17,7 @@ name = "jsrm" # Required # # For a discussion on single-sourcing the version, see # https://packaging.python.org/guides/single-sourcing-package-version/ -version = "0.0.12" # Required +version = "0.0.13" # Required # This is a one-line description or tagline of what your project does. This # corresponds to the "Summary" metadata field: diff --git a/src/jsrm/systems/planar_pcs.py b/src/jsrm/systems/planar_pcs.py index e4ff11e..54d84f5 100644 --- a/src/jsrm/systems/planar_pcs.py +++ b/src/jsrm/systems/planar_pcs.py @@ -19,6 +19,7 @@ def factory( strain_selector: Array = None, xi_eq: Optional[Array] = None, stiffness_fn: Optional[Callable] = None, + actuation_mapping_fn: Optional[Callable] = None, global_eps: float = 1e-6, ) -> Tuple[ Array, @@ -37,7 +38,9 @@ def factory( strain are active / non-zero xi_eq: array of shape (3 * num_segments) with the rest strains of the rod stiffness_fn: function to compute the stiffness matrix of the system. Should have the signature - stiffness_fn(params: Dict[str, Array], formulate_in_strain_space: bool) -> Array + stiffness_fn(params: Dict[str, Array], B_xi, formulate_in_strain_space: bool) -> Array + actuation_mapping_fn: function to compute the actuation matrix that maps the actuation space to the + configuration space. global_eps: small number to avoid singularities (e.g., division by zero) Returns: B_xi: strain basis matrix of shape (3 * num_segments, n_q) @@ -204,12 +207,13 @@ def classify_segment(params: Dict[str, Array], s: Array) -> Tuple[Array, Array]: if stiffness_fn is None: def stiffness_fn( - params: Dict[str, Array], formulate_in_strain_space: bool = False + params: Dict[str, Array], B_xi: Array, formulate_in_strain_space: bool = False ) -> Array: """ Compute the stiffness matrix of the system. Args: params: Dictionary of robot parameters + B_xi: Strain basis matrix formulate_in_strain_space: whether to formulate the elastic matrix in the strain space Returns: K: elastic matrix of shape (n_q, n_q) if formulate_in_strain_space is False or (n_xi, n_xi) otherwise @@ -232,6 +236,30 @@ def stiffness_fn( return K + if actuation_mapping_fn is None: + def actuation_mapping_fn( + forward_kinematics_fn: Callable, + jacobian_fn: Callable, + params: Dict[str, Array], + B_xi: Array, + q: Array, + ) -> Array: + """ + Returns the actuation matrix that maps the actuation space to the configuration space. + Assumes the fully actuated and identity actuation matrix. + Args: + forward_kinematics_fn: function to compute the forward kinematics + jacobian_fn: function to compute the Jacobian + params: dictionary with robot parameters + B_xi: strain basis matrix + q: configuration of the robot + Returns: + A: actuation matrix of shape (n_xi, n_xi) where n_xi is the number of strains. + """ + A = B_xi.T @ jnp.identity(n_xi) @ B_xi + + return A + @jit def forward_kinematics_fn( params: Dict[str, Array], q: Array, s: Array, eps: float = global_eps @@ -329,7 +357,9 @@ def dynamical_matrices_fn( xi_epsed = apply_eps_to_bend_strains(xi, eps) # compute the stiffness matrix - K = stiffness_fn(params, formulate_in_strain_space=True) + K = stiffness_fn(params, B_xi, formulate_in_strain_space=True) + # compute the actuation matrix + A = actuation_mapping_fn(forward_kinematics_fn, jacobian_fn, params, B_xi, q) # dissipative matrix from the parameters D = params.get("D", jnp.zeros((n_xi, n_xi))) @@ -346,7 +376,7 @@ def dynamical_matrices_fn( D = B_xi.T @ D @ B_xi # apply the strain basis to the actuation matrix - alpha = B_xi.T @ jnp.identity(n_xi) @ B_xi + alpha = A return B, C, G, K, D, alpha @@ -385,7 +415,7 @@ def potential_energy_fn( xi_epsed = apply_eps_to_bend_strains(xi, eps) # compute the stiffness matrix - K = stiffness_fn(params, formulate_in_strain_space=True) + K = stiffness_fn(params, B_xi, formulate_in_strain_space=True) # elastic energy U_K = (xi - xi_eq).T @ K @ (xi - xi_eq) # evaluate K(xi) = K @ xi @@ -408,7 +438,7 @@ def energy_fn(params: Dict[str, Array], q: Array, q_d: Array) -> Array: Returns: E: total energy of shape () """ - T = kinetic_energy_fn(params, q_d) + T = kinetic_energy_fn(params, q, q_d) U = potential_energy_fn(params, q) E = T + U @@ -499,6 +529,7 @@ def operational_space_dynamical_matrices_fn( "apply_eps_to_bend_strains": apply_eps_to_bend_strains, "classify_segment": classify_segment, "stiffness_fn": stiffness_fn, + "actuation_mapping_fn": actuation_mapping_fn, "jacobian_fn": jacobian_fn, "kinetic_energy_fn": kinetic_energy_fn, "potential_energy_fn": potential_energy_fn, diff --git a/src/jsrm/systems/pneumatic_planar_pcs.py b/src/jsrm/systems/pneumatic_planar_pcs.py new file mode 100644 index 0000000..2253cfe --- /dev/null +++ b/src/jsrm/systems/pneumatic_planar_pcs.py @@ -0,0 +1,190 @@ +__all__ = ["factory", "stiffness_fn"] +from jax import Array, vmap +import jax.numpy as jnp +from jsrm.math_utils import blk_diag +import numpy as onp +from typing import Callable, Dict, Optional, Tuple, Union + +from .planar_pcs import factory as planar_pcs_factory + +def factory( + num_segments: int, + *args, + segment_actuation_selector: Optional[Array] = None, + **kwargs +): + """ + Factory function for the planar PCS. + Args: + num_segments: number of segments + segment_actuation_selector: actuation selector for the segments as boolean array of shape (num_segments,) + True entries signify that the segment is actuated, False entries signify that the segment is passive + Returns: + """ + if segment_actuation_selector is None: + segment_actuation_selector = jnp.ones(num_segments, dtype=bool) + + # number of input pressures + actuation_dim = segment_actuation_selector.sum() * 2 + + # matrix that maps the (possibly) underactuated actuation space to a full actuation space + actuation_basis = jnp.zeros((2 * num_segments, actuation_dim)) + actuation_basis_cumsum = jnp.cumsum(segment_actuation_selector) + for i in range(num_segments): + j = int(actuation_basis_cumsum[i].item()) - 1 + if segment_actuation_selector[i].item() is True: + actuation_basis = actuation_basis.at[2 * i, j].set(1.0) + actuation_basis = actuation_basis.at[2 * i + 1, j + 1].set(1.0) + + def actuation_mapping_fn( + forward_kinematics_fn: Callable, + jacobian_fn: Callable, + params: Dict[str, Array], + B_xi: Array, + q: Array, + ) -> Array: + """ + Returns the actuation matrix that maps the actuation space to the configuration space. + Args: + forward_kinematics_fn: function to compute the forward kinematics + jacobian_fn: function to compute the Jacobian + params: dictionary with robot parameters + B_xi: strain basis matrix + q: configuration of the robot + Returns: + A: actuation matrix of shape (n_xi, n_act) where n_xi is the number of strains and + n_act is the number of actuators + """ + # all segment bases and tips + sms = jnp.concatenate([jnp.zeros((1,)), jnp.cumsum(params["l"])], axis=0) + + # compute the poses of all segment tips + chi_sms = vmap(forward_kinematics_fn, in_axes=(None, None, 0))(params, q, sms) + + # compute the Jacobian for all segment tips + J_sms = vmap(jacobian_fn, in_axes=(None, None, 0))(params, q, sms) + + def compute_actuation_matrix_for_segment( + r_cham_in: Array, r_cham_out: Array, varphi_cham: Array, + chi_pe: Array, chi_de: Array, + J_pe: Array, J_de: Array, + ) -> Array: + """ + Compute the actuation matrix for a single segment. + We assume that each segment contains four identical and symmetric pneumatic chambers with pressures + p1, p2, p3, and p4, where p1 and p3 are the right and left chamber pressures respectively, and + p2 and p4 are the back and front chamber pressures respectively. The front and back chambers + do not exert a level arm (i.e., a bending moment) on the segment. + We map the control inputs u1 and u2 as follows to the pressures: + p1 = u1 (right chamber) + p2 = (u1 + u2) / 2 + p3 = u2 (left chamber) + p4 = (u1 + u2) / 2 + + Args: + r_cham_in: inner radius of each segment chamber + r_cham_out: outer radius of each segment chamber + varphi_cham: sector angle of each segment chamber + chi_pe: pose of the proximal end (i.e., the base) of the segment as array of shape (3,) + chi_de: pose of the distal end (i.e., the tip) of the segment as array of shape (3,) + J_pe: Jacobian of the proximal end of the segment as array of shape (3, n_q) + J_de: Jacobian of the distal end of the segment as array of shape (3, n_q) + Returns: + A_sm: actuation matrix of shape (n_xi, 2) + """ + # orientation of the proximal and distal ends of the segment + th_pe, th_de = chi_pe[2], chi_de[2] + + # compute the area of each pneumatic chamber (we assume identical chambers within a segment) + A_cham = 0.5 * varphi_cham * (r_cham_out ** 2 - r_cham_in ** 2) + # compute the center of pressure of the pneumatic chamber + r_cop = ( + 2 / 3 * jnp.sinc(0.5 * varphi_cham) * (r_cham_out ** 3 - r_cham_in ** 3) / (r_cham_out ** 2 - r_cham_in ** 2) + ) + + # compute the actuation matrix that collects the contributions of the pneumatic chambers in the given segment + # first we consider the contribution of the distal end + A_sm_de = J_de.T @ jnp.array([ + [-2 * A_cham * jnp.sin(th_de), -2 * A_cham * jnp.sin(th_de)], + [2 * A_cham * jnp.cos(th_de), 2 * A_cham * jnp.cos(th_de)], + [A_cham * r_cop, -A_cham * r_cop] + ]) + # then, we consider the contribution of the proximal end + A_sm_pe = J_pe.T @ jnp.array([ + [2 * A_cham * jnp.sin(th_pe), 2 * A_cham * jnp.sin(th_pe)], + [-2 * A_cham * jnp.cos(th_pe), -2 * A_cham * jnp.cos(th_pe)], + [-A_cham * r_cop, A_cham * r_cop] + ]) + + # sum the contributions of the distal and proximal ends + A_sm = A_sm_de + A_sm_pe + + return A_sm + + A_sms = vmap(compute_actuation_matrix_for_segment)( + params["r_cham_in"], params["r_cham_out"], params["varphi_cham"], + chi_pe=chi_sms[:-1], chi_de=chi_sms[1:], + J_pe=J_sms[:-1], J_de=J_sms[1:], + ) + # we need to sum the contributions of the actuation of each segment + A = jnp.sum(A_sms, axis=0) + + # apply the actuation_basis + A = A @ actuation_basis + + return A + + return planar_pcs_factory( + *args, stiffness_fn=stiffness_fn, actuation_mapping_fn=actuation_mapping_fn, **kwargs + ) + +def _compute_stiffness_matrix_for_segment( + l: Array, r: Array, r_cham_in: Array, r_cham_out: Array, varphi_cham: Array, E: Array +): + # cross-sectional area [m²] of the material + A_mat = jnp.pi * r ** 2 + 2 * r_cham_in ** 2 * varphi_cham - 2 * r_cham_out ** 2 * varphi_cham + # second moment of area [m⁴] of the material + Ib_mat = jnp.pi * r ** 4 / 4 + r_cham_in ** 4 * varphi_cham / 2 - r_cham_out ** 4 * varphi_cham / 2 + # poisson ratio of the material + nu = 0.0 + # shear modulus + G = E / (2 * (1 + nu)) + + # bending stiffness [Nm²] + Sbe = Ib_mat * E * l + # shear stiffness [N] + Ssh = 4 / 3 * A_mat * G * l + # axial stiffness [N] + Sax = A_mat * E * l + + S = jnp.diag(jnp.stack([Sbe, Ssh, Sax], axis=0)) + + return S + +def stiffness_fn( + params: Dict[str, Array], + B_xi: Array, + formulate_in_strain_space: bool = False, +) -> Array: + """ + Compute the stiffness matrix of the system. + Args: + params: Dictionary of robot parameters + B_xi: Strain basis matrix + formulate_in_strain_space: whether to formulate the elastic matrix in the strain space + Returns: + K: elastic matrix of shape (n_q, n_q) if formulate_in_strain_space is False or (n_xi, n_xi) otherwise + """ + # stiffness matrix of shape (num_segments, 3, 3) + S = vmap( + _compute_stiffness_matrix_for_segment + )( + params["l"], params["r"], params["r_cham_in"], params["r_cham_out"], params["varphi_cham"], params["E"] + ) + # we define the elastic matrix of shape (n_xi, n_xi) as K(xi) = K @ xi where K is equal to + K = blk_diag(S) + + if not formulate_in_strain_space: + K = B_xi.T @ K @ B_xi + + return K \ No newline at end of file diff --git a/src/jsrm/systems/utils.py b/src/jsrm/systems/utils.py index b859456..f1850ae 100644 --- a/src/jsrm/systems/utils.py +++ b/src/jsrm/systems/utils.py @@ -121,6 +121,6 @@ def compute_planar_stiffness_matrix(l: Array, A: Array, Ib: Array, E: Array, G: Returns: S: stiffness matrix of shape (3, 3) """ - S = l* jnp.diag(jnp.stack([Ib * E, 4 / 3 * A * G, A * E], axis=0)) + S = l * jnp.diag(jnp.stack([Ib * E, 4 / 3 * A * G, A * E], axis=0)) return S