Skip to content

Commit

Permalink
Add end_effector_attached argument to functions generating hsa params
Browse files Browse the repository at this point in the history
  • Loading branch information
@mstoelzle
mstoelzle committed Oct 26, 2023
1 parent ef270dd commit c084130
Showing 1 changed file with 61 additions and 66 deletions.
127 changes: 61 additions & 66 deletions src/jsrm/parameters/hsa_params.py
View file
Original file line number Diff line number Diff line change
@@ -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]
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand All @@ -133,36 +156,41 @@ 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,
}

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 | {
Expand All @@ -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
Expand All @@ -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,
}
Expand All @@ -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(
Expand All @@ -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)

0 comments on commit c084130

Please sign in to comment.