def _compute_shear_stretch_strains(
position_collection,
volume,
lengths,
tangents,
radius,
rest_lengths,
rest_voronoi_lengths,
dilatation,
voronoi_dilatation,
director_collection,
sigma,
):
# Quick trick : Instead of evaliation Q(et-d^3), use property that Q*d3 = (0,0,1), a constant
_compute_all_dilatations(
position_collection,
volume,
lengths,
tangents,
radius,
dilatation,
rest_lengths,
rest_voronoi_lengths,
voronoi_dilatation,
)
z_vector = np.array([0.0, 0.0, 1.0]).reshape(3, -1)
sigma[:] = dilatation * _batch_matvec(director_collection,
tangents) - z_vector
def compute_shear_energy(self):
sigma_diff = self.sigma - self.rest_sigma
shear_internal_torques = _batch_matvec(self.shear_matrix, sigma_diff)
return (0.5 * (_batch_dot(sigma_diff, shear_internal_torques) *
self.rest_lengths).sum())
def compute_bending_energy(self):
kappa_diff = self.kappa - self.rest_kappa
bending_internal_torques = _batch_matvec(self.bend_matrix, kappa_diff)
return (0.5 * (_batch_dot(kappa_diff, bending_internal_torques) *
self.rest_voronoi_lengths).sum())
def test_update_acceleration(self, n_elem):
"""
In this test case, we initialize a straight rod.
We set correct parameters for rod mass, dilatation, mass moment of inertia
and call the function update_accelerations and compare the angular and
translational acceleration with the correct values.
This test case tests,
update_accelerations
_update_accelerations
"""
initial, test_rod = constructor(n_elem, nu=0.0)
mass = test_rod.mass
external_forces = np.zeros(3 * (n_elem + 1)).reshape(3, n_elem + 1)
external_torques = np.zeros(3 * n_elem).reshape(3, n_elem)
for i in range(0, n_elem):
external_torques[..., i] = np.random.rand(3)
for i in range(0, n_elem + 1):
external_forces[..., i] = np.random.rand(3)
test_rod.external_forces[:] = external_forces
test_rod.external_torques[:] = external_torques
# No dilatation in the rods
dilatations = np.ones(n_elem)
# Set mass moment of inertia matrix to identity matrix for convenience.
# Inverse of identity = identity
inv_mass_moment_of_inertia = np.repeat(np.identity(3)[:, :,
np.newaxis],
n_elem,
axis=2)
test_rod.inv_mass_second_moment_of_inertia[:] = inv_mass_moment_of_inertia
# Compute acceleration
test_rod.update_accelerations(time=0)
correct_acceleration = external_forces / mass
assert_allclose(
test_rod.acceleration_collection,
correct_acceleration,
atol=Tolerance.atol(),
)
correct_angular_acceleration = (
_batch_matvec(inv_mass_moment_of_inertia, external_torques) *
dilatations)
assert_allclose(
test_rod.alpha_collection,
correct_angular_acceleration,
atol=Tolerance.atol(),
)
def test_batch_matvec(blocksize):
input_matrix_collection = np.random.randn(3, 3, blocksize)
input_vector_collection = np.random.randn(3, blocksize)
test_vector_collection = _batch_matvec(input_matrix_collection,
input_vector_collection)
correct_vector_collection = [
np.dot(input_matrix_collection[..., i],
input_vector_collection[..., i]) for i in range(blocksize)
]
correct_vector_collection = np.array(correct_vector_collection).T
assert_allclose(test_vector_collection, correct_vector_collection)
def _compute_internal_bending_twist_stresses_from_model(
director_collection,
rest_voronoi_lengths,
internal_couple,
bend_matrix,
kappa,
rest_kappa,
):
"""
Linear force functional
Operates on
B : (3,3,n) tensor and curvature kappa (3,n)
Returns
-------
"""
_compute_bending_twist_strains(director_collection, rest_voronoi_lengths,
kappa) # concept : needs to compute kappa
internal_couple[:] = _batch_matvec(bend_matrix, kappa - rest_kappa)
def _compute_internal_shear_stretch_stresses_from_model(
position_collection,
volume,
lengths,
tangents,
radius,
rest_lengths,
rest_voronoi_lengths,
dilatation,
voronoi_dilatation,
director_collection,
sigma,
rest_sigma,
shear_matrix,
internal_stress,
):
"""
Linear force functional
Operates on
S : (3,3,n) tensor and sigma (3,n)
Returns
-------
"""
_compute_shear_stretch_strains(
position_collection,
volume,
lengths,
tangents,
radius,
rest_lengths,
rest_voronoi_lengths,
dilatation,
voronoi_dilatation,
director_collection,
sigma,
)
internal_stress[:] = _batch_matvec(shear_matrix, sigma - rest_sigma)
def _compute_internal_torques(
position_collection,
velocity_collection,
tangents,
lengths,
rest_lengths,
director_collection,
rest_voronoi_lengths,
bend_matrix,
rest_kappa,
kappa,
voronoi_dilatation,
mass_second_moment_of_inertia,
omega_collection,
internal_stress,
internal_couple,
dilatation,
dilatation_rate,
dissipation_constant_for_torques,
damping_torques,
internal_torques,
):
# Compute \tau_l and cache it using internal_couple
# Be careful about usage though
_compute_internal_bending_twist_stresses_from_model(
director_collection,
rest_voronoi_lengths,
internal_couple,
bend_matrix,
kappa,
rest_kappa,
)
# Compute dilatation rate when needed, dilatation itself is done before
# in internal_stresses
_compute_dilatation_rate(position_collection, velocity_collection, lengths,
rest_lengths, dilatation_rate)
# FIXME: change memory overload instead for the below calls!
voronoi_dilatation_inv_cube_cached = 1.0 / voronoi_dilatation**3
# Delta(\tau_L / \Epsilon^3)
bend_twist_couple_2D = difference_kernel(
internal_couple * voronoi_dilatation_inv_cube_cached)
# \mathcal{A}[ (\kappa x \tau_L ) * \hat{D} / \Epsilon^3 ]
bend_twist_couple_3D = quadrature_kernel(
_batch_cross(kappa, internal_couple) * rest_voronoi_lengths *
voronoi_dilatation_inv_cube_cached)
# (Qt x n_L) * \hat{l}
shear_stretch_couple = (_batch_cross(
_batch_matvec(director_collection, tangents), internal_stress) *
rest_lengths)
# I apply common sub expression elimination here, as J w / e is used in both the lagrangian and dilatation
# terms
# TODO : the _batch_matvec kernel needs to depend on the representation of J, and should be coded as such
J_omega_upon_e = (
_batch_matvec(mass_second_moment_of_inertia, omega_collection) /
dilatation)
# (J \omega_L / e) x \omega_L
# Warning : Do not do micro-optimization here : you can ignore dividing by dilatation as we later multiply by it
# but this causes confusion and violates SRP
lagrangian_transport = _batch_cross(J_omega_upon_e, omega_collection)
# Note : in the computation of dilatation_rate, there is an optimization opportunity as dilatation rate has
# a dilatation-like term in the numerator, which we cancel here
# (J \omega_L / e^2) . (de/dt)
unsteady_dilatation = J_omega_upon_e * dilatation_rate / dilatation
_compute_damping_torques(damping_torques, omega_collection,
dissipation_constant_for_torques, lengths)
blocksize = internal_torques.shape[1]
for i in range(3):
for k in range(blocksize):
internal_torques[i, k] = (bend_twist_couple_2D[i, k] +
bend_twist_couple_3D[i, k] +
shear_stretch_couple[i, k] +
lagrangian_transport[i, k] +
unsteady_dilatation[i, k] -
damping_torques[i, k])
def compute_rotational_energy(self):
J_omega_upon_e = (_batch_matvec(self.mass_second_moment_of_inertia,
self.omega_collection) /
self.dilatation)
return 0.5 * np.einsum("ik,ik->k", self.omega_collection,
J_omega_upon_e).sum()
def anisotropic_friction(
plane_origin,
plane_normal,
surface_tol,
slip_velocity_tol,
k,
nu,
kinetic_mu_forward,
kinetic_mu_backward,
kinetic_mu_sideways,
static_mu_forward,
static_mu_backward,
static_mu_sideways,
radius,
tangents,
position_collection,
director_collection,
velocity_collection,
omega_collection,
internal_forces,
external_forces,
internal_torques,
external_torques,
):
plane_response_force_mag, no_contact_point_idx = apply_normal_force_numba(
plane_origin,
plane_normal,
surface_tol,
k,
nu,
radius,
position_collection,
velocity_collection,
internal_forces,
external_forces,
)
# First compute component of rod tangent in plane. Because friction forces acts in plane not out of plane. Thus
# axial direction has to be in plane, it cannot be out of plane. We are projecting rod element tangent vector in
# to the plane. So friction forces can only be in plane forces and not out of plane.
tangent_along_normal_direction = _batch_product_i_ik_to_k(plane_normal, tangents)
tangent_perpendicular_to_normal_direction = tangents - _batch_product_i_k_to_ik(
plane_normal, tangent_along_normal_direction
)
tangent_perpendicular_to_normal_direction_mag = _batch_norm(
tangent_perpendicular_to_normal_direction
)
# Normalize tangent_perpendicular_to_normal_direction. This is axial direction for plane. Here we are adding
# small tolerance (1e-10) for normalization, in order to prevent division by 0.
axial_direction = _batch_product_k_ik_to_ik(
1 / (tangent_perpendicular_to_normal_direction_mag + 1e-14),
tangent_perpendicular_to_normal_direction,
)
element_velocity = node_to_element_pos_or_vel(velocity_collection)
# first apply axial kinetic friction
velocity_mag_along_axial_direction = _batch_dot(element_velocity, axial_direction)
velocity_along_axial_direction = _batch_product_k_ik_to_ik(
velocity_mag_along_axial_direction, axial_direction
)
# Friction forces depends on the direction of velocity, in other words sign
# of the velocity vector.
velocity_sign_along_axial_direction = np.sign(velocity_mag_along_axial_direction)
# Check top for sign convention
kinetic_mu = 0.5 * (
kinetic_mu_forward * (1 + velocity_sign_along_axial_direction)
+ kinetic_mu_backward * (1 - velocity_sign_along_axial_direction)
)
# Call slip function to check if elements slipping or not
slip_function_along_axial_direction = find_slipping_elements(
velocity_along_axial_direction, slip_velocity_tol
)
kinetic_friction_force_along_axial_direction = -(
(1.0 - slip_function_along_axial_direction)
* kinetic_mu
* plane_response_force_mag
* velocity_sign_along_axial_direction
* axial_direction
)
# If rod element does not have any contact with plane, plane cannot apply friction
# force on the element. Thus lets set kinetic friction force to 0.0 for the no contact points.
kinetic_friction_force_along_axial_direction[..., no_contact_point_idx] = 0.0
elements_to_nodes_inplace(
kinetic_friction_force_along_axial_direction, external_forces
)
# Now rolling kinetic friction
rolling_direction = _batch_vec_oneD_vec_cross(axial_direction, plane_normal)
torque_arm = _batch_product_i_k_to_ik(-plane_normal, radius)
velocity_along_rolling_direction = _batch_dot(element_velocity, rolling_direction)
directors_transpose = _batch_matrix_transpose(director_collection)
# w_rot = Q.T @ omega @ Q @ r
rotation_velocity = _batch_matvec(
directors_transpose,
_batch_cross(omega_collection, _batch_matvec(director_collection, torque_arm)),
)
rotation_velocity_along_rolling_direction = _batch_dot(
rotation_velocity, rolling_direction
)
slip_velocity_mag_along_rolling_direction = (
velocity_along_rolling_direction + rotation_velocity_along_rolling_direction
)
slip_velocity_along_rolling_direction = _batch_product_k_ik_to_ik(
slip_velocity_mag_along_rolling_direction, rolling_direction
)
slip_velocity_sign_along_rolling_direction = np.sign(
slip_velocity_mag_along_rolling_direction
)
slip_function_along_rolling_direction = find_slipping_elements(
slip_velocity_along_rolling_direction, slip_velocity_tol
)
kinetic_friction_force_along_rolling_direction = -(
(1.0 - slip_function_along_rolling_direction)
* kinetic_mu_sideways
* plane_response_force_mag
* slip_velocity_sign_along_rolling_direction
* rolling_direction
)
# If rod element does not have any contact with plane, plane cannot apply friction
# force on the element. Thus lets set kinetic friction force to 0.0 for the no contact points.
kinetic_friction_force_along_rolling_direction[..., no_contact_point_idx] = 0.0
elements_to_nodes_inplace(
kinetic_friction_force_along_rolling_direction, external_forces
)
# torque = Q @ r @ Fr
external_torques += _batch_matvec(
director_collection,
_batch_cross(torque_arm, kinetic_friction_force_along_rolling_direction),
)
# now axial static friction
nodal_total_forces = _batch_vector_sum(internal_forces, external_forces)
element_total_forces = nodes_to_elements(nodal_total_forces)
force_component_along_axial_direction = _batch_dot(
element_total_forces, axial_direction
)
force_component_sign_along_axial_direction = np.sign(
force_component_along_axial_direction
)
# check top for sign convention
static_mu = 0.5 * (
static_mu_forward * (1 + force_component_sign_along_axial_direction)
+ static_mu_backward * (1 - force_component_sign_along_axial_direction)
)
max_friction_force = (
slip_function_along_axial_direction * static_mu * plane_response_force_mag
)
# friction = min(mu N, pushing force)
static_friction_force_along_axial_direction = -(
np.minimum(np.fabs(force_component_along_axial_direction), max_friction_force)
* force_component_sign_along_axial_direction
* axial_direction
)
# If rod element does not have any contact with plane, plane cannot apply friction
# force on the element. Thus lets set static friction force to 0.0 for the no contact points.
static_friction_force_along_axial_direction[..., no_contact_point_idx] = 0.0
elements_to_nodes_inplace(
static_friction_force_along_axial_direction, external_forces
)
# now rolling static friction
# there is some normal, tangent and rolling directions inconsitency from Elastica
total_torques = _batch_matvec(
directors_transpose, (internal_torques + external_torques)
)
# Elastica has opposite defs of tangents in interaction.h and rod.cpp
total_torques_along_axial_direction = _batch_dot(total_torques, axial_direction)
force_component_along_rolling_direction = _batch_dot(
element_total_forces, rolling_direction
)
noslip_force = -(
(
radius * force_component_along_rolling_direction
- 2.0 * total_torques_along_axial_direction
)
/ 3.0
/ radius
)
max_friction_force = (
slip_function_along_rolling_direction
* static_mu_sideways
* plane_response_force_mag
)
noslip_force_sign = np.sign(noslip_force)
static_friction_force_along_rolling_direction = (
np.minimum(np.fabs(noslip_force), max_friction_force)
* noslip_force_sign
* rolling_direction
)
# If rod element does not have any contact with plane, plane cannot apply friction
# force on the element. Thus lets set plane static friction force to 0.0 for the no contact points.
static_friction_force_along_rolling_direction[..., no_contact_point_idx] = 0.0
elements_to_nodes_inplace(
static_friction_force_along_rolling_direction, external_forces
)
external_torques += _batch_matvec(
director_collection,
_batch_cross(torque_arm, static_friction_force_along_rolling_direction),
)