def __init__(self):
bs = 32
self.position_collection = np.random.randn(MaxDimension.value(), bs)
self.director_collection = np.random.randn(MaxDimension.value(),
MaxDimension.value(), bs)
self.velocity_collection = np.random.randn(MaxDimension.value(), bs)
self.omega_collection = np.random.randn(MaxDimension.value(), bs)
self.mass = np.abs(np.random.randn(bs))
self.external_forces = np.zeros(bs)
def test_director_if_tangent_and_d3_are_not_same():
"""
This test is checking the case if the tangent and d3 of the directors
are not equal to each other.
Returns
-------
"""
n_elems = 10
start = np.array([0.0, 0.0, 0.0])
direction = np.array([1.0, 0.0, 0.0])
normal = np.array([0.0, 0.0, 1.0])
base_length = 1.0
base_radius = 0.25
density = 1000
nu = 0.1
youngs_modulus = 1e6
poisson_ratio = 0.3
position = np.zeros((3, n_elems + 1))
end = start + direction * base_length
for i in range(0, 3):
position[i, ...] = np.linspace(start[i], end[i], n_elems + 1)
# Set the directors such that tangent and d3 are not same.
input_directors = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elems))
binormal = np.cross(direction, normal)
normal_collection = np.repeat(binormal[:, np.newaxis], n_elems, axis=1)
binormal_collection = np.repeat(normal[:, np.newaxis], n_elems, axis=1)
new_direction = np.cross(binormal, normal)
direction_collection = np.repeat(new_direction[:, np.newaxis],
n_elems,
axis=1)
input_directors[0, ...] = normal_collection
input_directors[1, ...] = binormal_collection
input_directors[2, ...] = direction_collection
MockRodForTest.straight_rod(
n_elems,
start,
direction,
normal,
base_length,
base_radius,
density,
nu,
youngs_modulus,
poisson_ratio,
position=position,
directors=input_directors,
)
def test_directors_using_input_position_array(n_elems):
"""
This test is testing the case for which directors are computed
using the input position array and user defined normal.
Parameters
----------
n_elems
Returns
-------
"""
start = np.array([0.0, 0.0, 0.0])
direction = np.array([1.0, 0.0, 0.0])
normal = np.array([0.0, 0.0, 1.0])
base_length = 1.0
base_radius = 0.25
density = 1000
nu = 0.1
youngs_modulus = 1e6
poisson_ratio = 0.3
# Check directors, give position as input and let allocate function to compute directors.
input_position = np.zeros((3, n_elems + 1))
input_position[0, :] = np.linspace(start[0], start[0] + base_length,
n_elems + 1)
correct_directors = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elems))
binormal = np.cross(direction, normal)
tangent_collection = np.repeat(direction[:, np.newaxis], n_elems, axis=1)
normal_collection = np.repeat(normal[:, np.newaxis], n_elems, axis=1)
binormal_collection = np.repeat(binormal[:, np.newaxis], n_elems, axis=1)
correct_directors[0, ...] = normal_collection
correct_directors[1, ...] = binormal_collection
correct_directors[2, ...] = tangent_collection
mockrod = MockRodForTest.straight_rod(
n_elems,
start,
direction,
normal,
base_length,
base_radius,
density,
nu,
youngs_modulus,
poisson_ratio,
position=input_position,
)
test_directors = mockrod.director_collection
assert_allclose(correct_directors, test_directors, atol=Tolerance.atol())
def test_case_shear_torque(self):
"""
In this test case we initialize a straight rod with two elements
and set bending matrix to zero. This gives us opportunity decouple
shear torque from twist and bending torques in internal torques
equation. Then we modify node positions of second element and
introduce artificial bending. Finally, we compute shear torque
using internal torque function and compare with analytical value.
This test case is for testing shear torque term,
in internal torques equation.
Tested function
_compute_internal_torques
"""
n_elem = 2
initial, test_rod = constructor(n_elem, nu=0.0)
position = np.zeros((MaxDimension.value(), n_elem + 1))
position[..., 0] = np.array([0.0, 0.0, 0.0])
position[..., 1] = np.array([0.0, 0.0, 0.5])
position[..., 2] = np.array([0.0, -0.3, 0.9])
test_rod.position_collection = position
# Simplify the computations, and chose shear matrix as identity matrix.
test_rod.shear_matrix[:] = np.repeat(np.identity(3)[:, :, np.newaxis],
n_elem - 1,
axis=2)
# Internal shear stress function is tested previously
test_rod._compute_internal_shear_stretch_stresses_from_model()
correct_shear_torques = np.zeros((MaxDimension.value(), n_elem))
# Correct shear torques can be computed easily.
# Procedure:
# 1) Q = [1., 0., 0.; 0., 1., 0.; 0., 0., 1.]
# 2) t = [0., -0.6, 0.8]
# 3) sigma = (eQt-d3) = [0.0, -0.6, -0.2]
# 4) Qt = [0., -0.6, 0.8]
# 5) torque = Qt x sigma
# Note that this is not generic, but it does not to be, it is testing the functions.
correct_shear_torques[..., -1] = np.array([0.3, 0.0, 0.0])
# Set bending matrix to zero matrix, because we dont want
# any contribution from bending on total internal torques
test_rod.bend_matrix[:] = 0.0
test_torques = test_rod._compute_internal_torques()
assert_allclose(test_torques,
correct_shear_torques,
atol=Tolerance.atol())
def __init__(self, n_elem):
"""
This class initialize a straight rod,
which is for testing interaction functions.
Parameters
----------
n_elem
"""
base_length = 1.0
direction = np.array([0.0, 0.0, 1.0])
start = np.array([0.0, 0.0, 0.0])
end = start + direction * base_length
self.n_elem = n_elem
self.position_collection = np.zeros((MaxDimension.value(), n_elem + 1))
for i in range(0, MaxDimension.value()):
self.position_collection[i, ...] = np.linspace(start[i],
end[i],
num=n_elem + 1)
self.director_collection = np.repeat(np.identity(3)[:, :, np.newaxis],
n_elem,
axis=2)
self.radius = np.repeat(np.array([0.25]), n_elem, axis=0)
self.tangents = np.repeat(direction[:, np.newaxis], n_elem, axis=1)
self.velocity_collection = np.zeros((MaxDimension.value(), n_elem + 1))
self.omega_collection = np.zeros((MaxDimension.value(), n_elem))
self.external_forces = np.zeros((MaxDimension.value(), n_elem + 1))
self.external_torques = np.zeros((MaxDimension.value(), n_elem))
self.internal_forces = np.zeros((MaxDimension.value(), n_elem + 1))
self.internal_torques = np.zeros((MaxDimension.value(), n_elem))
self.lengths = np.ones(n_elem) * base_length / n_elem
def __init__(
self,
elemental_position_collection,
dimension_collection,
elements_per_aabb: int,
):
"""
Doesn't differentiate tangent direction from the rest : potentially harmful as
maybe you don't need to expand to radius amount in tangential direction
:param position_collection:
:param dimension_collection:
:param elements_per_aabb:
"""
n_positions = elemental_position_collection.shape[1] # n_pos
self.n_aabb = n_positions // elements_per_aabb
self.n_aabb += (1 if (n_positions % elements_per_aabb) else 0
) # extra if not perfectly divisible
self.elements_per_aabb = elements_per_aabb
self.aabb = np.empty(
(MaxDimension.value(), 2, self.n_aabb)) # 2 for min and max
assert dimension_collection.shape[1] == n_positions, "bad"
# Initialize the aabbs posthaste
self.update(elemental_position_collection, dimension_collection)
def test_director_if_d3_cross_d2_notequal_to_d1():
"""
This test is checking the case if the directors, d3xd2 is not equal
to d1 and creates an AssertionError.
Returns
-------
"""
n_elems = 10
start = np.array([0.0, 0.0, 0.0])
direction = np.array([1.0, 0.0, 0.0])
normal = np.array([0.0, 0.0, 1.0])
base_length = 1.0
base_radius = 0.25
density = 1000
nu = 0.1
youngs_modulus = 1e6
poisson_ratio = 0.3
# Check directors, give directors as input and check their validity.
# Let the assertion fail by setting d3=d2 for the input director
input_directors = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elems))
binormal = np.cross(direction, normal)
normal_collection = np.repeat(normal[:, np.newaxis], n_elems, axis=1)
binormal_collection = np.repeat(binormal[:, np.newaxis], n_elems, axis=1)
input_directors[0, ...] = normal_collection
input_directors[1, ...] = binormal_collection
input_directors[2, ...] = binormal_collection
MockRodForTest.straight_rod(
n_elems,
start,
direction,
normal,
base_length,
base_radius,
density,
nu,
youngs_modulus,
poisson_ratio,
directors=input_directors,
)
def test_compute_directors_matrix_using_user_inputs(n_elems):
"""
This test checks the director array created by allocate function. For this
test case we use user defined direction, normal to compute directors.
Returns
-------
"""
start = np.array([0.0, 0.0, 0.0])
direction = np.array([1.0, 0.0, 0.0])
normal = np.array([0.0, 0.0, 1.0])
base_length = 1.0
base_radius = 0.25
density = 1000
nu = 0.1
youngs_modulus = 1e6
poisson_ratio = 0.3
# Check directors, if we dont input any directors, computed ones should be valid
correct_directors = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elems))
binormal = np.cross(direction, normal)
tangent_collection = np.repeat(direction[:, np.newaxis], n_elems, axis=1)
normal_collection = np.repeat(normal[:, np.newaxis], n_elems, axis=1)
binormal_collection = np.repeat(binormal[:, np.newaxis], n_elems, axis=1)
correct_directors[0, ...] = normal_collection
correct_directors[1, ...] = binormal_collection
correct_directors[2, ...] = tangent_collection
mockrod = MockRodForTest.straight_rod(
n_elems,
start,
direction,
normal,
base_length,
base_radius,
density,
nu,
youngs_modulus,
poisson_ratio,
)
test_directors = mockrod.director_collection
assert_allclose(correct_directors, test_directors, atol=Tolerance.atol())
def compute_geometry_analytically(n_elem):
initial = BaseClass(n_elem)
# Construct position array using start and direction vectors.
# This position array will be our reference for test cases
end = initial.start + initial.direction * initial.base_length
position = np.zeros((MaxDimension.value(), n_elem + 1))
for i in range(0, MaxDimension.value()):
position[i, ...] = np.linspace(initial.start[i],
end[i],
num=n_elem + 1)
# Compute geometry
# length of each element is same we dont need to use position array for calculation of lengths
rest_lengths = np.repeat(initial.base_length / n_elem, n_elem)
tangents = np.repeat(initial.direction[:, np.newaxis], n_elem, axis=1)
radius = np.repeat(initial.base_radius, n_elem)
return position, rest_lengths, tangents, radius
def compute_forces_analytically(n_elem, dilatation):
internal_stress = compute_stress_analytically(n_elem, dilatation)
# Internal forces in between elements have to be zero, because
# we compress every element by same amount. Thus we only need
# to compute forces at the first and last nodes. We know that
# forces at the first and last node have to be in opposite direction
# thus we multiply forces on last node with -1.0.
internal_forces = np.zeros((MaxDimension.value(), n_elem + 1))
internal_forces[..., 0] = internal_stress[..., 0] / dilatation
internal_forces[..., -1] = -1.0 * internal_stress[..., 0] / dilatation
return internal_forces
def make_from_aabb(cls, aabb_collection, scale_factor=4):
# Make position collection and dimension collection arrays from aabb_collection
# Wasted effort, but only once during construction
n_aabb_from_lower_level = len(aabb_collection)
elemental_position_collection = np.zeros(
(MaxDimension.value(), n_aabb_from_lower_level))
# (r,r,dl) in (d1,d2,d3) coordinates
dimension_collection = np.zeros(
(MaxDimension.value(), n_aabb_from_lower_level))
for idx, aabb in enumerate(aabb_collection):
# By design in the bottom level, there's only one AABB. So the last index is always 1
# Also asserting herre
assert aabb.n_aabb == 1, "Number of aabbs not 1"
elemental_position_collection[...,
idx] = 0.5 * (aabb.aabb[..., 0, 0] +
aabb.aabb[..., 1, 0])
dimension_collection[..., idx] = 0.5 * (aabb.aabb[..., 1, 0] -
aabb.aabb[..., 0, 0])
return cls(elemental_position_collection, dimension_collection,
scale_factor)
def _compute_internal_torques(self):
return np.zeros((MaxDimension.value(), self.n_elem))
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, k, nu):
super().__init__(k, nu)
# 0 is min, 1 is max
self.aabb_rod = np.empty((MaxDimension.value(), 2))
self.aabb_cylinder = np.empty((MaxDimension.value(), 2))
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 __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 test_case_bend_straight_rod(self, alpha):
"""
In this test case we initialize a straight rod with 2 elements
and numerically bend the rod. We modify node positions and directors
to make a isosceles triangle. Then first we compute curvature
between two elements and compute the angle between them.
Finally, we compute bend twist couples and compare with
correct solution.
This test function tests
_compute_bending_twist_strains
_compute_internal_torques
only bend_twist_couple terms.
"""
n_elem = 2
initial, test_rod = constructor(n_elem, nu=0.0)
base_length = initial.base_length
# Change the coordinates of nodes, artificially bend the rod.
# /\
# ------ ==> / \
# / \
# Here I chose a isosceles triangle.
length = base_length / n_elem
position = np.zeros((MaxDimension.value(), n_elem + 1))
position[..., 0] = np.array([0.0, 0.0, 0.0])
position[..., 1] = length * np.array(
[0.0, np.sin(alpha), np.cos(alpha)])
position[..., 2] = length * np.array([0.0, 0.0, 2 * np.cos(alpha)])
test_rod.position_collection = position
# Set the directors manually. This is easy since we have two elements.
directors = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elem))
directors[..., 0] = np.array((
[1.0, 0.0, 0.0],
[0.0, np.cos(alpha), -np.sin(alpha)],
[0.0, np.sin(alpha), np.cos(alpha)],
))
directors[..., -1] = np.array((
[1.0, 0.0, 0.0],
[0.0, np.cos(alpha), np.sin(alpha)],
[0, -np.sin(alpha), np.cos(alpha)],
))
test_rod.director_collection = directors
# Compute voronoi rest length. Since elements lengths are equal
# in this test case, rest voronoi length can be easily computed
# dividing base length to number of elements.
rest_voronoi_length = base_length / n_elem
# Now compute geometry and dilatation, which we need for curvature calculations.
_compute_all_dilatations(
test_rod.position_collection,
test_rod.volume,
test_rod.lengths,
test_rod.tangents,
test_rod.radius,
test_rod.dilatation,
test_rod.rest_lengths,
test_rod.rest_voronoi_lengths,
test_rod.voronoi_dilatation,
)
_compute_dilatation_rate(
test_rod.position_collection,
test_rod.velocity_collection,
test_rod.lengths,
test_rod.rest_lengths,
test_rod.dilatation_rate,
)
_compute_bending_twist_strains(test_rod.director_collection,
test_rod.rest_voronoi_lengths,
test_rod.kappa)
# Generalized rotation per unit length is given by rest_D_i * Kappa_i.
# Thus in order to get the angle between two elements, we need to multiply
# kappa with rest_D_i . But this will give the exterior vertex angle of the
# triangle. Think as, we rotate element 1 clockwise direction and align with
# the element 2.
#
# \
# /\ \ 1
# 1 / \ 2 ==> \
# / \ \
# \ 2
# \
#
# So for this transformation we use exterior vertex angle of isosceles triangle.
# Exterior vertex angle can be computed easily, it is the sum of base angles
# , since this is isosceles triangle it is 2*base_angle
correct_angle = np.degrees(
np.array([2 * alpha, 0.0, 0.0]).reshape(3, 1))
test_angle = np.degrees(test_rod.kappa * test_rod.rest_voronoi_lengths)
assert_allclose(test_angle, correct_angle, atol=Tolerance.atol())
# Now lets test bending stress terms in internal torques equation.
# Here we will test bend twist couple 2D and bend twist couple 3D terms of the
# internal torques equation. Set the bending matrix to identity matrix for simplification.
test_rod.bend_matrix[:] = np.repeat(np.identity(3)[:, :, np.newaxis],
n_elem - 1,
axis=2)
# We need to compute shear stress, for internal torque equation.
# Shear stress is not used in this test case. In order to make sure shear
# stress do not contribute to the total torque we use assert check.
_compute_internal_bending_twist_stresses_from_model(
test_rod.director_collection,
test_rod.rest_voronoi_lengths,
test_rod.internal_couple,
test_rod.bend_matrix,
test_rod.kappa,
test_rod.rest_kappa,
)
assert_allclose(
test_rod.internal_stress,
np.zeros(3 * n_elem).reshape(3, n_elem),
atol=Tolerance.atol(),
)
# Make sure voronoi dilatation is 1
assert_allclose(test_rod.voronoi_dilatation,
np.array([1.0]),
atol=Tolerance.atol())
# Compute correct torques, first compute correct kappa.
correct_kappa = np.radians(correct_angle / rest_voronoi_length)
# We only need to compute bend twist couple 2D term for comparison,
# because bend twist couple 3D term is already zero, due to cross product.
# TODO: Extended this test for multiple elements more than 2.
correct_torques = np.zeros((MaxDimension.value(), n_elem))
correct_torques[..., 0] = correct_kappa[..., 0]
correct_torques[..., -1] = -1.0 * correct_kappa[..., -1]
_compute_internal_torques(
test_rod.position_collection,
test_rod.velocity_collection,
test_rod.tangents,
test_rod.lengths,
test_rod.rest_lengths,
test_rod.director_collection,
test_rod.rest_voronoi_lengths,
test_rod.bend_matrix,
test_rod.rest_kappa,
test_rod.kappa,
test_rod.voronoi_dilatation,
test_rod.mass_second_moment_of_inertia,
test_rod.omega_collection,
test_rod.internal_stress,
test_rod.internal_couple,
test_rod.dilatation,
test_rod.dilatation_rate,
test_rod.dissipation_constant_for_torques,
test_rod.damping_torques,
test_rod.internal_torques,
test_rod.ghost_voronoi_idx,
)
assert_allclose(test_rod.internal_torques,
correct_torques,
atol=Tolerance.atol())
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)
# in-plane
horizontal_direction = np.array([0.0, 0.0, 1.0]).reshape(-1, 1)
vertical_direction = np.array([1.0, 0.0, 0.0]).reshape(-1, 1)
# out-of-plane
normal = np.array([0.0, 1.0, 0.0])
total_length = 3.0
base_radius = 0.25
base_area = np.pi * base_radius ** 2
density = 5000
nu = 0.0
youngs_modulus = 1e4
poisson_ratio = 0.5
positions = np.empty((MaxDimension.value(), n_elem + 1))
dl = total_length / n_elem
# First half of positions stem from slope angle_of_inclination
first_half = np.arange(half_n_elem + 1.0).reshape(1, -1)
positions[..., : half_n_elem + 1] = origin + dl * first_half * (
np.cos(angle_of_inclination) * horizontal_direction
+ np.sin(angle_of_inclination) * vertical_direction
)
positions[..., half_n_elem:] = positions[
..., half_n_elem : half_n_elem + 1
] + dl * first_half * (
np.cos(angle_of_inclination) * horizontal_direction
- np.sin(angle_of_inclination) * vertical_direction
)
def _compute_internal_forces(self):
return np.zeros((MaxDimension.value(), self.n_elem + 1))
def __init__(
self,
n_elements,
position,
directors,
rest_lengths,
density,
volume,
mass_second_moment_of_inertia,
nu,
*args,
**kwargs
):
"""
Parameters
----------
n_elements: int
position: numpy.ndarray
2D (dim, blocksize) array containing data with 'float' type.
Rod node position array.
directors: numpy.ndarray
3D (dim, dim, blocksize) array containing data with 'float' type.
Rod element directors array.
rest_lengths: numpy.ndarray
1D (blocksize) array containing data with 'float' type.
Rod element rest lengths.
density: numpy.ndarray
1D (blocksize) array containing data with 'float' type.
Rod element density.
volume: numpy.ndarray
1D (blocksize) array containing data with 'float' type.
Rod element volume.
mass_second_moment_of_inertia: numpy.ndarray
2D (dim, blocksize) array containing data with 'float' type.
Rod element mass second moment of inertia.
nu: numpy.ndarray
1D (blocksize) array containing data with 'float' type.
Rod element dissipation constant.
*args
Variable length argument list.
**kwargs
Arbitrary keyword arguments.
"""
velocities = np.zeros((MaxDimension.value(), n_elements + 1))
omegas = np.zeros((MaxDimension.value(), n_elements)) # + 1e-16
accelerations = 0.0 * velocities
angular_accelerations = 0.0 * omegas
self.n_elems = n_elements
self._vector_states = np.hstack(
(position, velocities, omegas, accelerations, angular_accelerations)
)
self._matrix_states = directors.copy()
# initial set to zero; if coming through kwargs then modify
self.rest_lengths = rest_lengths
self.density = density
self.volume = volume
self.mass = np.zeros(n_elements + 1)
self.mass[:-1] += 0.5 * self.density * self.volume
self.mass[1:] += 0.5 * self.density * self.volume
self.mass_second_moment_of_inertia = mass_second_moment_of_inertia
self.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()
)
self.inv_mass_second_moment_of_inertia[..., i] = np.linalg.inv(
mass_second_moment_of_inertia[..., i]
)
self.nu = nu
self.rest_voronoi_lengths = 0.5 * (
self.rest_lengths[1:] + self.rest_lengths[:-1]
)
# calculated in `_compute_internal_forces_and_torques`
self.internal_forces = 0 * accelerations
self.internal_torques = 0 * angular_accelerations
# will apply external force and torques externally
self.external_forces = 0 * accelerations
self.external_torques = 0 * angular_accelerations
# calculated in `compute_geometry_from_state`
self.lengths = NotImplemented
self.tangents = NotImplemented
self.radius = NotImplemented
# calculated in `compute_all_dilatatation`
self.dilatation = NotImplemented
self.voronoi_dilatation = NotImplemented
self.dilatation_rate = NotImplemented
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 straight_rod(
cls,
n_elements,
start,
direction,
normal,
base_length,
base_radius,
density,
nu,
youngs_modulus,
poisson_ratio,
alpha_c=4.0 / 3.0,
*args,
**kwargs
):
"""
Call this method to initialize and generate a Cosserat rod object that is a straight rod. Future versions will contain
methods for curvilinear rods.
Parameters
----------
n_elements: float
Rod number of elements.
start: numpy.ndarray
1D (dim) array containing data with 'float' type. Start position of the rod.
direction: numpy.ndarray
1D (dim) array containing data with 'float' type. Direction or tangent of the rod.
normal: numpy.ndarray
1D (dim) array containing data with 'float' type. Normal direction of the rod.
base_length: float
Initial length of the rod.
base_radius: float
Initial radius of the rod.
density: float
Density of the rod.
nu: float
Dissipation constant of the rod.
youngs_modulus: float
Youngs modulus of the rod.
poisson_ratio: float
Poisson ratio of the rod is used to compute shear modulus.
alpha_c: float
*args
Variable length argument list.
**kwargs
Arbitrary keyword arguments.
Returns
-------
"""
# FIXME: Make sure G=E/(poisson_ratio+1.0) in wikipedia it is different
# Shear Modulus
shear_modulus = youngs_modulus / (poisson_ratio + 1.0)
# 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 / n_elements
)
# Shear/Stretch matrix
shear_matrix = np.zeros(
(MaxDimension.value(), MaxDimension.value()), np.float64
)
np.fill_diagonal(
shear_matrix,
[
alpha_c * shear_modulus * A0,
alpha_c * shear_modulus * A0,
youngs_modulus * A0,
],
)
# Bend/Twist matrix
bend_matrix = np.zeros((MaxDimension.value(), MaxDimension.value()), np.float64)
np.fill_diagonal(
bend_matrix,
[youngs_modulus * I0_1, youngs_modulus * I0_2, shear_modulus * I0_3],
)
rod = _CosseratRodBase.straight_rod(
n_elements,
start,
direction,
normal,
base_length,
base_radius,
density,
nu,
mass_second_moment_of_inertia,
*args,
**kwargs
)
return cls(n_elements, shear_matrix, bend_matrix, rod, *args, **kwargs)
def test_case_compute_bending_energy(self, alpha, nu=0.0):
"""
Similar to the previous test case test_case_bend_straight_rod.
In this test case we initialize a straight rod with 2 elements
and numerically bend the rod. We modify node positions and directors
to make a isosceles triangle. Then first we compute curvature
between two elements and compute the angle between them.
Finally, we compute the bending energy of rod and compare with
correct solution.
This test function tests
compute_bending_energy
Parameters
----------
alpha
nu
Returns
-------
"""
n_elem = 2
initial, test_rod = constructor(n_elem, nu=0.0)
base_length = initial.base_length
# Change the coordinates of nodes, artificially bend the rod.
# /\
# ------ ==> / \
# / \
# Here I chose a isosceles triangle.
length = base_length / n_elem
position = np.zeros((MaxDimension.value(), n_elem + 1))
position[..., 0] = np.array([0.0, 0.0, 0.0])
position[..., 1] = length * np.array(
[0.0, np.sin(alpha), np.cos(alpha)])
position[..., 2] = length * np.array([0.0, 0.0, 2 * np.cos(alpha)])
test_rod.position_collection = position
# Set the directors manually. This is easy since we have two elements.
directors = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elem))
directors[..., 0] = np.array((
[1.0, 0.0, 0.0],
[0.0, np.cos(alpha), -np.sin(alpha)],
[0.0, np.sin(alpha), np.cos(alpha)],
))
directors[..., -1] = np.array((
[1.0, 0.0, 0.0],
[0.0, np.cos(alpha), np.sin(alpha)],
[0, -np.sin(alpha), np.cos(alpha)],
))
test_rod.director_collection = directors
# Compute voronoi rest length. Since elements lengths are equal
# in this test case, rest voronoi length can be easily computed
# dividing base length to number of elements.
rest_voronoi_length = base_length / n_elem
# Now compute geometry and dilatation, which we need for curvature calculations.
_compute_all_dilatations(
test_rod.position_collection,
test_rod.volume,
test_rod.lengths,
test_rod.tangents,
test_rod.radius,
test_rod.dilatation,
test_rod.rest_lengths,
test_rod.rest_voronoi_lengths,
test_rod.voronoi_dilatation,
)
_compute_dilatation_rate(
test_rod.position_collection,
test_rod.velocity_collection,
test_rod.lengths,
test_rod.rest_lengths,
test_rod.dilatation_rate,
)
_compute_bending_twist_strains(test_rod.director_collection,
test_rod.rest_voronoi_lengths,
test_rod.kappa)
# Generalized rotation per unit length is given by rest_D_i * Kappa_i.
# Thus in order to get the angle between two elements, we need to multiply
# kappa with rest_D_i . But this will give the exterior vertex angle of the
# triangle. Think as, we rotate element 1 clockwise direction and align with
# the element 2.
#
# \
# /\ \ 1
# 1 / \ 2 ==> \
# / \ \
# \ 2
# \
#
# So for this transformation we use exterior vertex angle of isosceles triangle.
# Exterior vertex angle can be computed easily, it is the sum of base angles
# , since this is isosceles triangle it is 2*base_angle
correct_angle = np.degrees(
np.array([2 * alpha, 0.0, 0.0]).reshape(3, 1))
test_angle = np.degrees(test_rod.kappa * test_rod.rest_voronoi_lengths)
assert_allclose(test_angle, correct_angle, atol=Tolerance.atol())
# Now lets test bending stress terms in internal torques equation.
# Here we will test bend twist couple 2D and bend twist couple 3D terms of the
# internal torques equation. Set the bending matrix to identity matrix for simplification.
test_rod.bend_matrix[:] = np.repeat(np.identity(3)[:, :, np.newaxis],
n_elem - 1,
axis=2)
# Compute bending energy
correct_kappa = 2 * alpha / rest_voronoi_length
correct_bending_energy = (0.5 * correct_kappa * correct_kappa *
rest_voronoi_length)
test_bending_energy = test_rod.compute_bending_energy()
assert_allclose(test_bending_energy,
correct_bending_energy,
atol=Tolerance.atol())
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)