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 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 test_batch_dot(blocksize):
input_first_vector_collection = np.random.randn(3, blocksize)
input_second_vector_collection = np.random.randn(3, blocksize)
test_vector_collection = _batch_dot(input_first_vector_collection,
input_second_vector_collection)
correct_vector_collection = np.einsum("ij,ij->j",
input_first_vector_collection,
input_second_vector_collection)
assert_allclose(test_vector_collection, correct_vector_collection)
def _compute_dilatation_rate(position_collection, velocity_collection, lengths,
rest_lengths, dilatation_rate):
"""
Returns
-------
"""
# TODO Use the vector formula rather than separating it out
# self.lengths = l_i = |r^{i+1} - r^{i}|
r_dot_v = _batch_dot(position_collection, velocity_collection)
r_plus_one_dot_v = _batch_dot(position_collection[..., 1:],
velocity_collection[..., :-1])
r_dot_v_plus_one = _batch_dot(position_collection[..., :-1],
velocity_collection[..., 1:])
blocksize = lengths.shape[0]
for k in range(blocksize):
dilatation_rate[k] = ((r_dot_v[k] + r_dot_v[k + 1] -
r_dot_v_plus_one[k] - r_plus_one_dot_v[k]) /
lengths[k] / rest_lengths[k])
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),
)