From ae1b67d4321ca04810d32f3de7193cc27a776885 Mon Sep 17 00:00:00 2001 From: Maximilian Stolzle Date: Tue, 19 Nov 2024 18:28:55 -0500 Subject: [PATCH] Properly implement `actuation_basis` --- examples/simulate_pneumatic_planar_pcs.py | 19 +++++++++++++------ src/jsrm/systems/pneumatic_planar_pcs.py | 11 +++++++++-- 2 files changed, 22 insertions(+), 8 deletions(-) diff --git a/examples/simulate_pneumatic_planar_pcs.py b/examples/simulate_pneumatic_planar_pcs.py index e899f7d..b9825d1 100644 --- a/examples/simulate_pneumatic_planar_pcs.py +++ b/examples/simulate_pneumatic_planar_pcs.py @@ -47,6 +47,18 @@ # 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 = auxiliary_fns["actuation_mapping_fn"] + +def sweep_actuation_mapping(): + q = jnp.zeros((2 * num_segments,)) + A = actuation_mapping_fn(params, B_xi, q) + print("A =\n", A) + def simulate_robot(): # define initial configuration @@ -59,12 +71,6 @@ def simulate_robot(): sim_dt = 5e-5 # simulation time step ts = jnp.arange(0.0, 2, dt) # time steps - strain_basis, forward_kinematics_fn, dynamical_matrices_fn, auxiliary_fns = ( - pneumatic_planar_pcs.factory(sym_exp_filepath, strain_selector) - ) - # jit the functions - dynamical_matrices_fn = jax.jit(partial(dynamical_matrices_fn)) - x0 = jnp.concatenate([q0, jnp.zeros_like(q0)]) # initial condition tau = jnp.zeros_like(q0) # torques @@ -160,4 +166,5 @@ def simulate_robot(): plt.show() if __name__ == "__main__": + sweep_actuation_mapping() simulate_robot() diff --git a/src/jsrm/systems/pneumatic_planar_pcs.py b/src/jsrm/systems/pneumatic_planar_pcs.py index 69b2a92..826d8f9 100644 --- a/src/jsrm/systems/pneumatic_planar_pcs.py +++ b/src/jsrm/systems/pneumatic_planar_pcs.py @@ -3,7 +3,7 @@ import jax.numpy as jnp from jsrm.math_utils import blk_diag import numpy as onp -from typing import Dict, Tuple, Union +from typing import Dict, Optional, Tuple, Union from .planar_pcs import factory as planar_pcs_factory @@ -16,6 +16,7 @@ def factory( """ 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: @@ -27,7 +28,13 @@ def factory( actuation_dim = segment_actuation_selector.sum() * 2 # matrix that maps the (possibly) underactuated actuation space to a full actuation space - actuation_basis = jnp.eye(num_segments)[segment_actuation_selector] + 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( params: Dict[str, Array],