def _compute_internal_torques(self):
# Compute \tau_l and cache it using internal_couple
# Be careful about usage though
self._compute_internal_bending_twist_stresses_from_model()
# Compute dilatation rate when needed, dilatation itself is done before
# in internal_stresses
self._compute_dilatation_rate()
# FIXME: change memory overload instead for the below calls!
voronoi_dilatation_inv_cube_cached = 1.0 / self.voronoi_dilatation**3
# Delta(\tau_L / \Epsilon^3)
bend_twist_couple_2D = difference_kernel(
self.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(self.kappa, self.internal_couple) *
self.rest_voronoi_lengths * voronoi_dilatation_inv_cube_cached)
# (Qt x n_L) * \hat{l}
shear_stretch_couple = (_batch_cross(
_batch_matvec(self.director_collection, self.tangents),
self.internal_stress,
) * self.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(self.mass_second_moment_of_inertia,
self.omega_collection) /
self.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,
self.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 * self.dilatation_rate / self.dilatation
# Compute damping torques
self._compute_damping_torques()
return (bend_twist_couple_2D + bend_twist_couple_3D +
shear_stretch_couple + lagrangian_transport +
unsteady_dilatation - self.damping_torques)
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 update_accelerations(self, time):
"""TODO Do we need to make the collection members abstract?
Parameters
----------
time
Returns
-------
"""
np.copyto(
self.acceleration_collection,
(self.internal_forces + self.external_forces) / self.mass,
)
np.copyto(
self.alpha_collection,
_batch_matvec(
self.inv_mass_second_moment_of_inertia,
(self.internal_torques + self.external_torques),
) * self.dilatation,
)
# Reset forces and torques
self.external_forces *= 0.0
self.external_torques *= 0.0
def _compute_shear_stretch_strains(self):
# Quick trick : Instead of evaliation Q(et-d^3), use property that Q*d3 = (0,0,1), a constant
self._compute_all_dilatations()
# FIXME: change memory overload instead for the below calls!
self.sigma = (self.dilatation *
_batch_matvec(self.director_collection, self.tangents) -
_get_z_vector())
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
"""
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 _compute_internal_bending_twist_stresses_from_model(self):
"""
Linear force functional
Operates on
B : (3,3,n) tensor and curvature kappa (3,n)
Returns
-------
"""
self._compute_bending_twist_strains(
) # concept : needs to compute kappa
self.internal_couple = _batch_matvec(self.bend_matrix,
self.kappa - self.rest_kappa)
def _compute_internal_shear_stretch_stresses_from_model(self):
"""
Linear force functional
Operates on
S : (3,3,n) tensor and sigma (3,n)
Returns
-------
"""
self._compute_shear_stretch_strains(
) # concept : needs to compute sigma
self.internal_stress = _batch_matvec(self.shear_matrix,
self.sigma - self.rest_sigma)
def test_batch_matvec(dim, blocksize):
for iteration in range(3):
input_matrix_collection = np.random.randn(dim, dim, blocksize)
input_vector_collection = np.random.randn(dim, 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_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 apply_forces(self, system, time=0.0):
# calculate axial and rolling directions
plane_response_force_mag, no_contact_point_idx = self.apply_normal_force(system)
normal_plane_collection = np.repeat(
self.plane_normal.reshape(3, 1), plane_response_force_mag.shape[0], axis=1
)
# 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 = np.einsum(
"ij, ij->j", system.tangents, normal_plane_collection
)
tangent_perpendicular_to_normal_direction = system.tangents - np.einsum(
"j, ij->ij", tangent_along_normal_direction, normal_plane_collection
)
tangent_perpendicular_to_normal_direction_mag = np.einsum(
"ij, ij->j",
tangent_perpendicular_to_normal_direction,
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 = np.einsum(
"ij, j-> ij",
tangent_perpendicular_to_normal_direction,
1 / (tangent_perpendicular_to_normal_direction_mag + 1e-14),
)
element_velocity = 0.5 * (
system.velocity_collection[..., :-1] + system.velocity_collection[..., 1:]
)
# first apply axial kinetic friction
velocity_mag_along_axial_direction = np.einsum(
"ij,ij->j", element_velocity, axial_direction
)
velocity_along_axial_direction = np.einsum(
"j, ij->ij", 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 * (
self.kinetic_mu_forward * (1 + velocity_sign_along_axial_direction)
+ self.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, self.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
system.external_forces[..., :-1] += (
0.5 * kinetic_friction_force_along_axial_direction
)
system.external_forces[..., 1:] += (
0.5 * kinetic_friction_force_along_axial_direction
)
# Now rolling kinetic friction
rolling_direction = _batch_cross(axial_direction, normal_plane_collection)
torque_arm = -system.radius * normal_plane_collection
velocity_along_rolling_direction = np.einsum(
"ij ,ij ->j ", element_velocity, rolling_direction
)
directors_transpose = np.einsum("ijk -> jik", system.director_collection)
# w_rot = Q.T @ omega @ Q @ r
rotation_velocity = _batch_matvec(
directors_transpose,
_batch_cross(
system.omega_collection,
_batch_matvec(system.director_collection, torque_arm),
),
)
rotation_velocity_along_rolling_direction = np.einsum(
"ij,ij->j", rotation_velocity, rolling_direction
)
slip_velocity_mag_along_rolling_direction = (
velocity_along_rolling_direction + rotation_velocity_along_rolling_direction
)
slip_velocity_along_rolling_direction = np.einsum(
"j, ij->ij", 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, self.slip_velocity_tol
)
kinetic_friction_force_along_rolling_direction = -(
(1.0 - slip_function_along_rolling_direction)
* self.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
system.external_forces[..., :-1] += (
0.5 * kinetic_friction_force_along_rolling_direction
)
system.external_forces[..., 1:] += (
0.5 * kinetic_friction_force_along_rolling_direction
)
# torque = Q @ r @ Fr
system.external_torques += _batch_matvec(
system.director_collection,
_batch_cross(torque_arm, kinetic_friction_force_along_rolling_direction),
)
# now axial static friction
nodal_total_forces = system.internal_forces + system.external_forces
element_total_forces = nodes_to_elements(nodal_total_forces)
force_component_along_axial_direction = np.einsum(
"ij,ij->j", 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 * (
self.static_mu_forward * (1 + force_component_sign_along_axial_direction)
+ self.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
system.external_forces[..., :-1] += (
0.5 * static_friction_force_along_axial_direction
)
system.external_forces[..., 1:] += (
0.5 * static_friction_force_along_axial_direction
)
# now rolling static friction
# there is some normal, tangent and rolling directions inconsitency from Elastica
total_torques = _batch_matvec(
directors_transpose, (system.internal_torques + system.external_torques)
)
# Elastica has opposite defs of tangents in interaction.h and rod.cpp
total_torques_along_axial_direction = np.einsum(
"ij,ij->j", total_torques, axial_direction
)
force_component_along_rolling_direction = np.einsum(
"ij,ij->j", element_total_forces, rolling_direction
)
noslip_force = -(
(
system.radius * force_component_along_rolling_direction
- 2.0 * total_torques_along_axial_direction
)
/ 3.0
/ system.radius
)
max_friction_force = (
slip_function_along_rolling_direction
* self.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
system.external_forces[..., :-1] += (
0.5 * static_friction_force_along_rolling_direction
)
system.external_forces[..., 1:] += (
0.5 * static_friction_force_along_rolling_direction
)
system.external_torques += _batch_matvec(
system.director_collection,
_batch_cross(torque_arm, static_friction_force_along_rolling_direction),
)