From c084130f71b00ef249484966e70332b404d71057 Mon Sep 17 00:00:00 2001 From: Maximilian Stolzle Date: Thu, 26 Oct 2023 23:33:39 +0200 Subject: [PATCH] Add `end_effector_attached` argument to functions generating hsa params --- src/jsrm/parameters/hsa_params.py | 127 ++++++++++++++---------------- 1 file changed, 61 insertions(+), 66 deletions(-) diff --git a/src/jsrm/parameters/hsa_params.py b/src/jsrm/parameters/hsa_params.py index d1e2daa..23d28c0 100644 --- a/src/jsrm/parameters/hsa_params.py +++ b/src/jsrm/parameters/hsa_params.py @@ -1,16 +1,24 @@ -__all__ = ["generate_base_params_for_fpu", "PARAMS_FPU_CONTROL", "PARAMS_FPU_SYSTEM_ID"] +__all__ = [ + "generate_base_params_for_fpu", "generate_base_params_for_epu", + "PARAMS_FPU_CONTROL", "PARAMS_FPU_SYSTEM_ID", + "PARAMS_EPU_CONTROL", "PARAMS_EPU_SYSTEM_ID", +] from jax import Array import jax.numpy as jnp from typing import Dict -def generate_common_base_params(num_segments: int, num_rods_per_segment) -> Dict[str, Array]: +def generate_common_base_params( + num_segments: int = 1, + num_rods_per_segment: int = 4, + end_effector_attached: int = False +) -> Dict[str, Array]: assert num_rods_per_segment % 2 == 0, "num_rods_per_segment must be even" ones_rod = jnp.ones((num_segments, num_rods_per_segment)) - return { + params = { "th0": jnp.array(0.0), # initial orientation angle [rad] "l": 59e-3 * jnp.ones((num_segments,)), # length of each rod [m] # length of the rigid proximal caps of the rods connecting to the base [m] @@ -41,9 +49,24 @@ def generate_common_base_params(num_segments: int, num_rods_per_segment) -> Dict "chiee_off": jnp.array([0.0, 0.0, 0.0]), } + if end_effector_attached: + # the end-effector is moved by 25mm in the y-dir relative to the top surface of the HSA platform + params["chiee_off"] = jnp.array([0.0, 0.025, 0.0]) + params["mpl"] = jnp.array(0.018) # the end-effector attachment has a mass of 18g + # the end-effector attachment has a center of gravity of 3.63mm in y-dir from its base. + # as it has a thickness of 25mm, this is -21.37mm from the top surface (i.e., end-effector position) + params["CoGpl"] = jnp.array([0.0, -0.02137]) + + return params -def generate_base_params_for_fpu(num_segments: int = 1, num_rods_per_segment: int = 2) -> Dict[str, Array]: - common_params = generate_common_base_params(num_segments, num_rods_per_segment) + +def generate_base_params_for_fpu( + num_segments: int = 1, + num_rods_per_segment: int = 4, + rod_multiplier: int = 1, + **kwargs +) -> Dict[str, Array]: + common_params = generate_common_base_params(num_segments, num_rods_per_segment, **kwargs) ones_rod = jnp.ones((num_segments, num_rods_per_segment)) # old params (1st ISER submission) @@ -121,7 +144,7 @@ def generate_base_params_for_fpu(num_segments: int = 1, num_rods_per_segment: in "rin": (25.4e-3 / 2 - 2.43e-3) * ones_rod, # this is for FPU rods # mass of FPU rod: 14 g # For FPU, this corresponds to a measure volume of 0000175355 m^3 --> rho = 798.38 kg/m^3 - "rhor": 798.38 * ones_rod, # Volumetric density of rods [kg/m^3], + "rhor": 798.38 * rod_multiplier * ones_rod, # Volumetric density of rods [kg/m^3], # Volumetric density of platform [kg/m^3], # weight of platform + marker holder + cylinder top piece: 0.107 kg # subtracting 4 x 9g for distal cap: 0.071 kg @@ -133,27 +156,27 @@ def generate_base_params_for_fpu(num_segments: int = 1, num_rods_per_segment: in # mass of the rigid end piece (up to rin): 9g # volume: pi*lpc*rout^2 = 0.0000126677 m^3 # --> rho = 710.4 kg/m^3 - "rhoec": 710.4 * jnp.ones((num_segments,)), + "rhoec": 710.4 * rod_multiplier * jnp.ones((num_segments,)), # Nominal bending stiffness of each rod [Nm^2] - "S_b_hat": 5.71346377e-04 * ones_rod, + "S_b_hat": 5.71346377e-04 * rod_multiplier * ones_rod, # Nominal shear stiffness of each rod [N] - "S_sh_hat": 5.91462074e-01 * ones_rod, + "S_sh_hat": 5.91462074e-01 * rod_multiplier * ones_rod, # Nominal axial stiffness of each rod [N] - "S_a_hat": 5.66472469 * ones_rod, + "S_a_hat": 5.66472469 * rod_multiplier * ones_rod, # Elastic coupling between bending and shear [Nm/rad] - "S_b_sh": 4.48419541e-03 * ones_rod, + "S_b_sh": 4.48419541e-03 * rod_multiplier * ones_rod, # Scaling of bending stiffness with twist strain [Nm^3/rad] - "C_S_b": -9.67560524e-06 * ones_rod, + "C_S_b": -9.67560524e-06 * rod_multiplier * ones_rod, # Scaling of shear stiffness with twist strain [Nm/rad] - "C_S_sh": -4.75687961e-04 * ones_rod, + "C_S_sh": -4.75687961e-04 * rod_multiplier * ones_rod, # Scaling of axial stiffness with twist strain [Nm/rad] - "C_S_a": 0.01508165 * ones_rod, + "C_S_a": 0.01508165 * rod_multiplier * ones_rod, # damping coefficient for bending of shape (num_segments, rods_per_segment) - "zetab": 2e-6 * ones_rod, + "zetab": 2e-6 * rod_multiplier * ones_rod, # damping coefficient for shear of shape (num_segments, rods_per_segment) - "zetash": 1e-5 * ones_rod, + "zetash": 1e-5 * rod_multiplier * ones_rod, # damping coefficient for axial elongation of shape (num_segments, rods_per_segment) - "zetaa": 8e-4 * ones_rod, + "zetaa": 8e-4 * rod_multiplier * ones_rod, # maximum twist angles (positive) [rad] "phi_max": 200 / 180 * jnp.pi * ones_rod, } @@ -161,8 +184,13 @@ def generate_base_params_for_fpu(num_segments: int = 1, num_rods_per_segment: in return params -def generate_base_params_for_epu(num_segments: int = 1, num_rods_per_segment: int = 2) -> Dict[str, Array]: - common_params = generate_common_base_params(num_segments, num_rods_per_segment) +def generate_base_params_for_epu( + num_segments: int = 1, + num_rods_per_segment: int = 4, + rod_multiplier: int = 1, + **kwargs +) -> Dict[str, Array]: + common_params = generate_common_base_params(num_segments, num_rods_per_segment, **kwargs) ones_rod = jnp.ones((num_segments, num_rods_per_segment)) params = common_params | { @@ -172,7 +200,7 @@ def generate_base_params_for_epu(num_segments: int = 1, num_rods_per_segment: in "rin": (25.4e-3 / 2 - 4.76e-3) * ones_rod, # this is for EPU rods # mass of EPU rod: 26 g # For EPU, this corresponds to a measure volume of 0000314034 m^3 --> rho = 827.94 kg/m^3 - "rhor": 827.94 * ones_rod, # Volumetric density of rods [kg/m^3], + "rhor": 827.94 * rod_multiplier * ones_rod, # Volumetric density of rods [kg/m^3], # Volumetric density of platform [kg/m^3], # weight of platform + marker holder + cylinder top piece: 0.107 kg # subtracting 4 x 9g for distal cap: 0.071 kg @@ -184,27 +212,27 @@ def generate_base_params_for_epu(num_segments: int = 1, num_rods_per_segment: in # mass of the rigid end piece (up to rin): 9g # volume: pi*lpc*rout^2 = 0.0000126677 m^3 # --> rho = 710.4 kg/m^3 - "rhoec": 710.4 * jnp.ones((num_segments,)), + "rhoec": 710.4 * rod_multiplier * jnp.ones((num_segments,)), # Nominal bending stiffness of each rod [Nm^2] - "S_b_hat": -2.53938133e-05 * ones_rod, + "S_b_hat": -2.53938133e-05 * rod_multiplier * ones_rod, # Nominal shear stiffness of each rod [N] - "S_sh_hat": 4.28135773e-02 * ones_rod, + "S_sh_hat": 4.28135773e-02 * rod_multiplier * ones_rod, # Nominal axial stiffness of each rod [N] - "S_a_hat": 0.73610293 * ones_rod, + "S_a_hat": 0. * rod_multiplier * ones_rod, # Elastic coupling between bending and shear [Nm/rad] - "S_b_sh": 5.04204068e-04 * ones_rod, + "S_b_sh": 5.04204068e-04 * rod_multiplier * ones_rod, # Scaling of bending stiffness with twist strain [Nm^3/rad] - "C_S_b": 3.90666351e-07 * ones_rod, + "C_S_b": 3.90666351e-07 * rod_multiplier * ones_rod, # Scaling of shear stiffness with twist strain [Nm/rad] - "C_S_sh": 2.93344701e-03 * ones_rod, + "C_S_sh": 2.93344701e-03 * rod_multiplier * ones_rod, # Scaling of axial stiffness with twist strain [Nm/rad] - "C_S_a": 0.00981059 * ones_rod, + "C_S_a": 0.00981059 * rod_multiplier * ones_rod, # damping coefficient for bending of shape (num_segments, rods_per_segment) - "zetab": 4e-6 * ones_rod, + "zetab": 4e-6 * rod_multiplier * ones_rod, # damping coefficient for shear of shape (num_segments, rods_per_segment) - "zetash": 8e-5 * ones_rod, + "zetash": 8e-5 * rod_multiplier * ones_rod, # damping coefficient for axial elongation of shape (num_segments, rods_per_segment) - "zetaa": 5e-3 * ones_rod, + "zetaa": 5e-3 * rod_multiplier * ones_rod, # maximum twist angles (positive) [rad] "phi_max": 270 / 180 * jnp.pi * ones_rod, } @@ -219,24 +247,7 @@ def generate_base_params_for_epu(num_segments: int = 1, num_rods_per_segment: in "roff": 24e-3 * jnp.array([[1.0, 1.0, -1.0, -1.0]]), } ) - -PARAMS_FPU_CONTROL = generate_base_params_for_fpu(num_segments=1, num_rods_per_segment=2) -PARAMS_FPU_CONTROL.update( - { - "rhor": 2 * PARAMS_FPU_CONTROL["rhor"], - "rhoec": 2 * PARAMS_FPU_CONTROL["rhoec"], - "S_b_hat": 2 * PARAMS_FPU_CONTROL["S_b_hat"], - "S_sh_hat": 2 * PARAMS_FPU_CONTROL["S_sh_hat"], - "S_a_hat": 2 * PARAMS_FPU_CONTROL["S_a_hat"], - "S_b_sh": 2 * PARAMS_FPU_CONTROL["S_b_sh"], - "C_S_b": 2 * PARAMS_FPU_CONTROL["C_S_b"], - "C_S_sh": 2 * PARAMS_FPU_CONTROL["C_S_sh"], - "C_S_a": 2 * PARAMS_FPU_CONTROL["C_S_a"], - "zetab": 2 * PARAMS_FPU_CONTROL["zetab"], - "zetash": 2 * PARAMS_FPU_CONTROL["zetash"], - "zetaa": 2 * PARAMS_FPU_CONTROL["zetaa"], - } -) +PARAMS_FPU_CONTROL = generate_base_params_for_fpu(num_segments=1, num_rods_per_segment=2, rod_multiplier=2) PARAMS_EPU_SYSTEM_ID = generate_base_params_for_epu(num_segments=1, num_rods_per_segment=4) PARAMS_EPU_SYSTEM_ID.update( @@ -246,20 +257,4 @@ def generate_base_params_for_epu(num_segments: int = 1, num_rods_per_segment: in } ) -PARAMS_EPU_CONTROL = generate_base_params_for_epu(num_segments=1, num_rods_per_segment=2) -PARAMS_EPU_CONTROL.update( - { - "rhor": 2 * PARAMS_EPU_CONTROL["rhor"], - "rhoec": 2 * PARAMS_EPU_CONTROL["rhoec"], - "S_b_hat": 2 * PARAMS_EPU_CONTROL["S_b_hat"], - "S_sh_hat": 2 * PARAMS_EPU_CONTROL["S_sh_hat"], - "S_a_hat": 2 * PARAMS_EPU_CONTROL["S_a_hat"], - "S_b_sh": 2 * PARAMS_EPU_CONTROL["S_b_sh"], - "C_S_b": 2 * PARAMS_EPU_CONTROL["C_S_b"], - "C_S_sh": 2 * PARAMS_EPU_CONTROL["C_S_sh"], - "C_S_a": 2 * PARAMS_EPU_CONTROL["C_S_a"], - "zetab": 2 * PARAMS_EPU_CONTROL["zetab"], - "zetash": 2 * PARAMS_EPU_CONTROL["zetash"], - "zetaa": 2 * PARAMS_EPU_CONTROL["zetaa"], - } -) +PARAMS_EPU_CONTROL = generate_base_params_for_epu(num_segments=1, num_rods_per_segment=2, rod_multiplier=2)