def __init__(self, center, base_radius, density):
# rigid body does not have elements it only have one node. We are setting n_elems to
# zero for only make code to work. _bootstrap_from_data requires n_elems to be defined
self.n_elems = 1
self.radius = base_radius
self.density = density
self.length = 2 * base_radius
# This is for a rigid body cylinder
self.volume = 4.0 / 3.0 * np.pi * base_radius**3
self.mass = np.array([self.volume * self.density])
self.normal = np.array([1.0, 0.0, 0.0]).reshape(3, 1)
self.tangents = np.array([0.0, 0.0, 1.0]).reshape(3, 1)
self.binormal = _batch_cross(self.tangents, self.normal)
# Mass second moment of inertia for disk cross-section
mass_second_moment_of_inertia = np.zeros(
(MaxDimension.value(), MaxDimension.value()), np.float64)
np.fill_diagonal(mass_second_moment_of_inertia,
2.0 / 5.0 * self.mass * self.radius**2)
self.inv_mass_second_moment_of_inertia = np.linalg.inv(
mass_second_moment_of_inertia).reshape(MaxDimension.value(),
MaxDimension.value(), 1)
# position is at the center
position = np.zeros((MaxDimension.value(), 1))
position[:, 0] = center
velocities = np.zeros((MaxDimension.value(), 1))
omegas = np.zeros((MaxDimension.value(), 1))
accelerations = 0.0 * velocities
angular_accelerations = 0.0 * omegas
directors = np.zeros((MaxDimension.value(), MaxDimension.value(), 1))
directors[0, ...] = self.normal
directors[1, ...] = _batch_cross(self.tangents, self.normal)
directors[2, ...] = self.tangents
# directors[0, ...] = [[1.0], [0.0], [0.0]]
# directors[1, ...] = [[0.0], [1.0], [0.0]]
# directors[2, ...] = [[0.0], [0.0], [1.0]]
self._vector_states = np.hstack((position, velocities, omegas,
accelerations, angular_accelerations))
self._matrix_states = directors.copy()
self.internal_forces = np.zeros(
(MaxDimension.value())).reshape(MaxDimension.value(), 1)
self.internal_torques = np.zeros(
(MaxDimension.value())).reshape(MaxDimension.value(), 1)
self.external_forces = np.zeros(
(MaxDimension.value())).reshape(MaxDimension.value(), 1)
self.external_torques = np.zeros(
(MaxDimension.value())).reshape(MaxDimension.value(), 1)
_RigidRodSymplecticStepperMixin.__init__(self)
def __init__(self, start, direction, normal, base_length, base_radius,
density):
# rigid body does not have elements it only have one node. We are setting n_elems to
# zero for only make code to work. _bootstrap_from_data requires n_elems to be defined
self.n_elems = 1
normal = normal.reshape(3, 1)
tangents = direction.reshape(3, 1)
binormal = _batch_cross(tangents, normal)
self.radius = base_radius
self.length = base_length
self.density = density
# This is for a rigid body cylinder
self.volume = np.pi * base_radius * base_radius * base_length
self.mass = np.array([self.volume * self.density])
# Second moment of inertia
A0 = np.pi * base_radius * base_radius
I0_1 = A0 * A0 / (4.0 * np.pi)
I0_2 = I0_1
I0_3 = 2.0 * I0_2
I0 = np.array([I0_1, I0_2, I0_3])
# Mass second moment of inertia for disk cross-section
mass_second_moment_of_inertia = np.zeros(
(MaxDimension.value(), MaxDimension.value()), np.float64)
np.fill_diagonal(mass_second_moment_of_inertia,
I0 * density * base_length)
self.mass_second_moment_of_inertia = mass_second_moment_of_inertia.reshape(
MaxDimension.value(), MaxDimension.value(), 1)
self.inv_mass_second_moment_of_inertia = np.linalg.inv(
mass_second_moment_of_inertia).reshape(MaxDimension.value(),
MaxDimension.value(), 1)
# position is at the center
self.position_collection = np.zeros((MaxDimension.value(), 1))
self.position_collection[:] = (
start.reshape(3, 1) + direction.reshape(3, 1) * base_length / 2)
self.velocity_collection = np.zeros((MaxDimension.value(), 1))
self.omega_collection = np.zeros((MaxDimension.value(), 1))
self.acceleration_collection = 0.0 * self.velocity_collection
self.alpha_collection = 0.0 * self.omega_collection
self.director_collection = np.zeros(
(MaxDimension.value(), MaxDimension.value(), 1))
self.director_collection[0, ...] = normal
self.director_collection[1, ...] = binormal
self.director_collection[2, ...] = tangents
self.external_forces = np.zeros(
(MaxDimension.value())).reshape(MaxDimension.value(), 1)
self.external_torques = np.zeros(
(MaxDimension.value())).reshape(MaxDimension.value(), 1)
def test_batch_cross(dim, blocksize):
input_first_vector_collection = np.random.randn(dim, blocksize)
input_second_vector_collection = np.random.randn(dim, blocksize)
test_vector_collection = _batch_cross(input_first_vector_collection,
input_second_vector_collection)
correct_vector_collection = np.cross(input_first_vector_collection,
input_second_vector_collection,
axisa=0,
axisb=0).T
assert_allclose(test_vector_collection, correct_vector_collection)
def __init__(self, center, base_radius, density):
# rigid body does not have elements it only have one node. We are setting n_elems to
# zero for only make code to work. _bootstrap_from_data requires n_elems to be defined
self.n_elems = 1
self.radius = base_radius
self.density = density
self.length = 2 * base_radius
# This is for a rigid body cylinder
self.volume = 4.0 / 3.0 * np.pi * base_radius**3
self.mass = np.array([self.volume * self.density])
normal = np.array([1.0, 0.0, 0.0]).reshape(3, 1)
tangents = np.array([0.0, 0.0, 1.0]).reshape(3, 1)
binormal = _batch_cross(tangents, normal)
# Mass second moment of inertia for disk cross-section
mass_second_moment_of_inertia = np.zeros(
(MaxDimension.value(), MaxDimension.value()), np.float64)
np.fill_diagonal(mass_second_moment_of_inertia,
2.0 / 5.0 * self.mass * self.radius**2)
self.mass_second_moment_of_inertia = mass_second_moment_of_inertia.reshape(
MaxDimension.value(), MaxDimension.value(), 1)
self.inv_mass_second_moment_of_inertia = np.linalg.inv(
mass_second_moment_of_inertia).reshape(MaxDimension.value(),
MaxDimension.value(), 1)
# position is at the center
self.position_collection = np.zeros((MaxDimension.value(), 1))
self.position_collection[:, 0] = center
self.velocity_collection = np.zeros((MaxDimension.value(), 1))
self.omega_collection = np.zeros((MaxDimension.value(), 1))
self.acceleration_collection = 0.0 * self.velocity_collection
self.alpha_collection = 0.0 * self.omega_collection
self.director_collection = np.zeros(
(MaxDimension.value(), MaxDimension.value(), 1))
self.director_collection[0, ...] = normal
self.director_collection[1, ...] = binormal
self.director_collection[2, ...] = tangents
self.external_forces = np.zeros(
(MaxDimension.value())).reshape(MaxDimension.value(), 1)
self.external_torques = np.zeros(
(MaxDimension.value())).reshape(MaxDimension.value(), 1)
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,
ghost_voronoi_idx,
):
# 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_for_block_structure(
internal_couple * voronoi_dilatation_inv_cube_cached,
ghost_voronoi_idx)
# \mathcal{A}[ (\kappa x \tau_L ) * \hat{D} / \Epsilon^3 ]
bend_twist_couple_3D = quadrature_kernel_for_block_structure(
_batch_cross(kappa, internal_couple) * rest_voronoi_lengths *
voronoi_dilatation_inv_cube_cached,
ghost_voronoi_idx,
)
# (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 allocate(n_elements,
start,
direction,
normal,
base_length,
base_radius,
density,
nu,
youngs_modulus,
poisson_ratio,
alpha_c=4.0 / 3.0,
*args,
**kwargs):
# sanity checks here
assert n_elements > 1
assert base_length > Tolerance.atol()
assert np.sqrt(np.dot(normal, normal)) > Tolerance.atol()
assert np.sqrt(np.dot(direction, direction)) > Tolerance.atol()
# Set the position array
position = np.zeros((MaxDimension.value(), n_elements + 1))
# check if position is in kwargs, if it is use user defined position otherwise generate position
if kwargs.__contains__("position"):
position_temp = np.array(kwargs["position"])
# Check the shape of the input position
assert position_temp.shape == (MaxDimension.value(), n_elements + 1), (
"Given position shape is not correct, it should be " +
str(position.shape) + " but instead " + str(position_temp.shape))
# Check if the start position of the rod and first entry of position array are the same
assert_allclose(
position_temp[..., 0],
start,
atol=Tolerance.atol(),
err_msg=str("First entry of position" + " (" +
str(position_temp[..., 0]) + " ) "
" is different than start " + " (" + str(start) +
" ) "),
)
position = position_temp.copy()
else:
end = start + direction * base_length
for i in range(0, 3):
position[i, ...] = np.linspace(start[i], end[i], n_elements + 1)
# Compute rest lengths and tangents
position_diff = position[..., 1:] - position[..., :-1]
rest_lengths = _batch_norm(position_diff)
tangents = position_diff / rest_lengths
normal /= np.linalg.norm(normal)
# Set the directors matrix
directors = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elements))
# check if directors is in kwargs, if it use user defined directors otherwise generate directors
if kwargs.__contains__("directors"):
directors_temp = np.array(kwargs["directors"])
# Check the shape of input directors
assert directors_temp.shape == (
MaxDimension.value(),
MaxDimension.value(),
n_elements,
), (" Given directors shape is not correct, it should be " +
str(directors.shape) + " but instead " + str(directors_temp.shape))
# Check if d1, d2, d3 are unit vectors
d1 = directors_temp[0, ...]
d2 = directors_temp[1, ...]
d3 = directors_temp[2, ...]
assert_allclose(
_batch_norm(d1),
np.ones((n_elements)),
atol=Tolerance.atol(),
err_msg=(
" d1 vector of input director matrix is not unit vector "),
)
assert_allclose(
_batch_norm(d2),
np.ones((n_elements)),
atol=Tolerance.atol(),
err_msg=(
" d2 vector of input director matrix is not unit vector "),
)
assert_allclose(
_batch_norm(d3),
np.ones((n_elements)),
atol=Tolerance.atol(),
err_msg=(
" d3 vector of input director matrix is not unit vector "),
)
# Check if d3xd1 = d2
assert_allclose(
_batch_cross(d3, d1),
d2,
atol=Tolerance.atol(),
err_msg=(" d3 x d1 != d2 of input director matrix"),
)
# Check if computed tangents from position is the same with d3
assert_allclose(
tangents,
d3,
atol=Tolerance.atol(),
err_msg=
" Tangent vector computed using node positions is different than d3 vector of input directors",
)
directors[:] = directors_temp[:]
else:
# Construct directors using tangents and normal
normal_collection = np.repeat(normal[:, np.newaxis],
n_elements,
axis=1)
# Check if rod normal and rod tangent are perpendicular to each other otherwise
# directors will be wrong!!
assert_allclose(
_batch_dot(normal_collection, tangents),
0,
atol=Tolerance.atol(),
err_msg=(
" Rod normal and tangent are not perpendicular to each other!"
),
)
directors[0, ...] = normal_collection
directors[1, ...] = _batch_cross(tangents, normal_collection)
directors[2, ...] = tangents
# Set radius array
radius = np.zeros((n_elements))
# Check if the user input radius is valid
radius_temp = np.array(base_radius)
assert radius_temp.ndim < 2, ("Input radius shape is not correct " +
str(radius_temp.shape) + " It should be " +
str(radius.shape) +
" or single floating number ")
radius[:] = radius_temp
# Check if the elements of radius are greater than tolerance
for k in range(n_elements):
assert radius[k] > Tolerance.atol(), (
" Radius has to be greater than 0" + " Check you radius input!")
# Set density array
density_array = np.zeros((n_elements))
# Check if the user input density is valid
density_temp = np.array(density)
assert density_temp.ndim < 2, ("Input density shape is not correct " +
str(density_temp.shape) + " It should be " +
str(density_array.shape) +
" or single floating number ")
density_array[:] = density_temp
# Check if the elements of density are greater than tolerance
for k in range(n_elements):
assert density_array[k] > Tolerance.atol(), (
" Density has to be greater than 0" + " Check you density input!")
# Second moment of inertia
A0 = np.pi * radius * radius
I0_1 = A0 * A0 / (4.0 * np.pi)
I0_2 = I0_1
I0_3 = 2.0 * I0_2
I0 = np.array([I0_1, I0_2, I0_3]).transpose()
# Mass second moment of inertia for disk cross-section
mass_second_moment_of_inertia = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elements), np.float64)
mass_second_moment_of_inertia_temp = np.einsum("ij,i->ij", I0,
density * rest_lengths)
for i in range(n_elements):
np.fill_diagonal(
mass_second_moment_of_inertia[..., i],
mass_second_moment_of_inertia_temp[i, :],
)
# sanity check of mass second moment of inertia
for k in range(n_elements):
for i in range(0, MaxDimension.value()):
assert mass_second_moment_of_inertia[i, i, k] > Tolerance.atol()
# Inverse of second moment of inertia
inv_mass_second_moment_of_inertia = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elements))
for i in range(n_elements):
# Check rank of mass moment of inertia matrix to see if it is invertible
assert (np.linalg.matrix_rank(
mass_second_moment_of_inertia[..., i]) == MaxDimension.value())
inv_mass_second_moment_of_inertia[..., i] = np.linalg.inv(
mass_second_moment_of_inertia[..., i])
# Shear/Stretch matrix
shear_modulus = youngs_modulus / (poisson_ratio + 1.0)
shear_matrix = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elements), np.float64)
for i in range(n_elements):
np.fill_diagonal(
shear_matrix[..., i],
[
alpha_c * shear_modulus * A0[i],
alpha_c * shear_modulus * A0[i],
youngs_modulus * A0[i],
],
)
# Bend/Twist matrix
bend_matrix = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elements), np.float64)
for i in range(n_elements):
np.fill_diagonal(
bend_matrix[..., i],
[
youngs_modulus * I0_1[i],
youngs_modulus * I0_2[i],
shear_modulus * I0_3[i],
],
)
for k in range(n_elements):
for i in range(0, MaxDimension.value()):
assert bend_matrix[i, i, k] > Tolerance.atol()
# Compute bend matrix in Voronoi Domain
bend_matrix = (bend_matrix[..., 1:] * rest_lengths[1:] +
bend_matrix[..., :-1] * rest_lengths[0:-1]) / (
rest_lengths[1:] + rest_lengths[:-1])
# Compute volume of elements
volume = np.pi * radius**2 * rest_lengths
# Compute mass of elements
mass = np.zeros(n_elements + 1)
mass[:-1] += 0.5 * density * volume
mass[1:] += 0.5 * density * volume
# Set dissipation constant or nu array
dissipation_constant_for_forces = np.zeros((n_elements))
# Check if the user input nu is valid
nu_temp = np.array(nu)
assert nu_temp.ndim < 2, (
"Input dissipation constant(nu) for forces shape is not correct " +
str(nu_temp.shape) + " It should be " +
str(dissipation_constant_for_forces.shape) +
" or single floating number ")
dissipation_constant_for_forces[:] = nu
# Check if the elements of dissipation constant greater than tolerance
for k in range(n_elements):
assert dissipation_constant_for_forces[k] >= 0.0, (
" Dissipation constant has to be equal or greater than 0 " +
" Check your dissipation constant(nu) input!")
dissipation_constant_for_torques = np.zeros((n_elements))
if kwargs.__contains__("nu_for_torques"):
temp_nu_for_torques = np.array(kwargs["nu_for_torques"])
assert temp_nu_for_torques.ndim < 2, (
"Input dissipation constant(nu) for torques shape is not correct "
+ str(temp_nu_for_torques.shape) + " It should be " +
str(dissipation_constant_for_torques.shape) +
" or single floating number ")
dissipation_constant_for_torques[:] = temp_nu_for_torques
else:
dissipation_constant_for_torques[:] = dissipation_constant_for_forces
# Generate rest sigma and rest kappa, use user input if defined
# set rest strains and curvature to be zero at start
# if found in kwargs modify (say for curved rod)
rest_sigma = np.zeros((MaxDimension.value(), n_elements))
if kwargs.__contains__("rest_sigma"):
temp_rest_sigma = np.array(kwargs["rest_sigma"])
assert temp_rest_sigma.shape == rest_sigma.shape, (
"Input rest sigma shape is not correct " +
str(temp_rest_sigma.shape) + " It should be " +
str(rest_sigma.shape))
rest_sigma[:] = temp_rest_sigma
rest_kappa = np.zeros((MaxDimension.value(), n_elements - 1))
if kwargs.__contains__("rest_kappa"):
temp_rest_kappa = np.array(kwargs["rest_kappa"])
assert temp_rest_kappa.shape == rest_kappa.shape, (
"Input rest kappa shape is not correct " +
str(temp_rest_kappa.shape) + " It should be " +
str(rest_kappa.shape))
rest_kappa[:] = temp_rest_kappa
# Compute rest voronoi length
rest_voronoi_lengths = 0.5 * (rest_lengths[1:] + rest_lengths[:-1])
# Allocate arrays for Cosserat Rod equations
velocities = np.zeros((MaxDimension.value(), n_elements + 1))
omegas = np.zeros((MaxDimension.value(), n_elements))
accelerations = 0.0 * velocities
angular_accelerations = 0.0 * omegas
_vector_states = np.hstack(
(position, velocities, omegas, accelerations, angular_accelerations))
_matrix_states = directors.copy()
internal_forces = 0.0 * accelerations
internal_torques = 0.0 * angular_accelerations
external_forces = 0.0 * accelerations
external_torques = 0.0 * angular_accelerations
lengths = np.zeros((n_elements))
tangents = np.zeros((3, n_elements))
dilatation = np.zeros((n_elements))
voronoi_dilatation = np.zeros((n_elements - 1))
dilatation_rate = np.zeros((n_elements))
sigma = np.zeros((3, n_elements))
kappa = np.zeros((3, n_elements - 1))
internal_stress = np.zeros((3, n_elements))
internal_couple = np.zeros((3, n_elements - 1))
damping_forces = np.zeros((3, n_elements + 1))
damping_torques = np.zeros((3, n_elements))
return (
n_elements,
_vector_states,
_matrix_states,
radius,
mass_second_moment_of_inertia,
inv_mass_second_moment_of_inertia,
shear_matrix,
bend_matrix,
density,
volume,
mass,
dissipation_constant_for_forces,
dissipation_constant_for_torques,
internal_forces,
internal_torques,
external_forces,
external_torques,
lengths,
rest_lengths,
tangents,
dilatation,
dilatation_rate,
voronoi_dilatation,
rest_voronoi_lengths,
sigma,
kappa,
rest_sigma,
rest_kappa,
internal_stress,
internal_couple,
damping_forces,
damping_torques,
)
def __init__(self, start, direction, normal, base_length, base_radius,
density):
# rigid body does not have elements it only have one node. We are setting n_elems to
# zero for only make code to work. _bootstrap_from_data requires n_elems to be defined
self.n_elems = 1
self.normal = normal.reshape(3, 1)
self.tangents = direction.reshape(3, 1)
self.binormal = np.cross(direction, normal).reshape(3, 1)
self.radius = base_radius
self.length = base_length
self.density = density
# This is for a rigid body cylinder
self.volume = np.pi * base_radius * base_radius * base_length
self.mass = np.array([self.volume * self.density])
# Second moment of inertia
A0 = np.pi * base_radius * base_radius
I0_1 = A0 * A0 / (4.0 * np.pi)
I0_2 = I0_1
I0_3 = 2.0 * I0_2
I0 = np.array([I0_1, I0_2, I0_3])
# Mass second moment of inertia for disk cross-section
mass_second_moment_of_inertia = np.zeros(
(MaxDimension.value(), MaxDimension.value()), np.float64)
np.fill_diagonal(mass_second_moment_of_inertia,
I0 * density * base_length)
self.inv_mass_second_moment_of_inertia = np.linalg.inv(
mass_second_moment_of_inertia).reshape(MaxDimension.value(),
MaxDimension.value(), 1)
# position is at the center
position = np.zeros((MaxDimension.value(), 1))
position[:] = start.reshape(
3, 1) + direction.reshape(3, 1) * base_length / 2
velocities = np.zeros((MaxDimension.value(), 1))
omegas = np.zeros((MaxDimension.value(), 1))
accelerations = 0.0 * velocities
angular_accelerations = 0.0 * omegas
directors = np.zeros((MaxDimension.value(), MaxDimension.value(), 1))
directors[0, ...] = self.normal
directors[1, ...] = _batch_cross(self.tangents, self.normal)
directors[2, ...] = self.tangents
self._vector_states = np.hstack((position, velocities, omegas,
accelerations, angular_accelerations))
self._matrix_states = directors.copy()
self.internal_forces = np.zeros(
(MaxDimension.value())).reshape(MaxDimension.value(), 1)
self.internal_torques = np.zeros(
(MaxDimension.value())).reshape(MaxDimension.value(), 1)
self.external_forces = np.zeros(
(MaxDimension.value())).reshape(MaxDimension.value(), 1)
self.external_torques = np.zeros(
(MaxDimension.value())).reshape(MaxDimension.value(), 1)
_RigidRodSymplecticStepperMixin.__init__(self)
def _compute_internal_torques(self):
"""
This method computes internal torques acting on elements.
Returns
-------
"""
# 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()
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
return (
bend_twist_couple_2D
+ bend_twist_couple_3D
+ shear_stretch_couple
+ lagrangian_transport
+ unsteady_dilatation
- self._compute_damping_torques()
)
def straight_rod(
cls,
n_elements,
start,
direction,
normal,
base_length,
base_radius,
density,
nu,
mass_second_moment_of_inertia,
*args,
**kwargs
):
# sanity checks here
assert n_elements > 1
assert base_length > Tolerance.atol()
assert base_radius > Tolerance.atol()
assert density > Tolerance.atol()
assert nu >= 0.0
assert np.sqrt(np.dot(normal, normal)) > Tolerance.atol()
assert np.sqrt(np.dot(direction, direction)) > Tolerance.atol()
for i in range(0, MaxDimension.value()):
assert mass_second_moment_of_inertia[i, i] > Tolerance.atol()
end = start + direction * base_length
position = np.zeros((MaxDimension.value(), n_elements + 1))
for i in range(0, MaxDimension.value()):
position[i, ...] = np.linspace(start[i], end[i], num=n_elements + 1)
# compute rest lengths and tangents
position_diff = position[..., 1:] - position[..., :-1]
rest_lengths = np.sqrt(np.einsum("ij,ij->j", position_diff, position_diff))
tangents = position_diff / rest_lengths
normal /= np.sqrt(np.dot(normal, normal))
# set directors
# check this order once
directors = np.zeros((MaxDimension.value(), MaxDimension.value(), n_elements))
normal_collection = np.repeat(normal[:, np.newaxis], n_elements, axis=1)
directors[0, ...] = normal_collection
directors[1, ...] = _batch_cross(tangents, normal_collection)
directors[2, ...] = tangents
volume = np.pi * base_radius ** 2 * rest_lengths
inertia_collection = np.repeat(
mass_second_moment_of_inertia[:, :, np.newaxis], n_elements, axis=2
)
# create rod
return cls(
n_elements,
position,
directors,
rest_lengths,
density,
volume,
inertia_collection,
nu,
*args,
**kwargs
)
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)
# 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_function_along_rolling_direction = find_slipping_elements(
slip_velocity_along_rolling_direction, slip_velocity_tol)
# Compute unitized total slip velocity vector. We will use this to distribute the weight of the rod in axial
# and rolling directions.
unitized_total_velocity = (slip_velocity_along_rolling_direction +
velocity_along_axial_direction)
unitized_total_velocity /= _batch_norm(unitized_total_velocity + 1e-14)
# Apply kinetic friction in axial direction.
kinetic_friction_force_along_axial_direction = -(
(1.0 - slip_function_along_axial_direction) * kinetic_mu *
plane_response_force_mag *
_batch_dot(unitized_total_velocity, 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)
# Apply kinetic friction in rolling direction.
kinetic_friction_force_along_rolling_direction = -(
(1.0 - slip_function_along_rolling_direction) *
kinetic_mu_sideways * plane_response_force_mag * _batch_dot(
unitized_total_velocity, 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),
)
