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), )