From c0b0b27bcc2bd5d6e13d1b5233fe0b57f2d69f16 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 6 Aug 2022 07:41:32 +0200 Subject: [PATCH] Fix #122 by enabling joints to connect rods and rigid bodies (#149) * Prepare `joint_cases_processing` for adding cylinders to joint cases examples * Fix bug of missing rotation of external torque into local frame for `FreeJoint` * Implement tests for testing generalized implementation of new `FreeJoint` - `GenericSystemTypeFreeJoint` and `GenericSystemTypeFixedJoint` experimental feature. * Fix specification of link direction of system two for `HingeJoint` * Tune elastic torque constant for `HingeJoint` example * Add cylinder to `spherical_joint.py` example * Update examples/JointCases/experimental/generic_system_type_spherical_joint.py * Update examples/JointCases/experimental/generic_system_type_fixed_joint.py * Update filenames of saved results from examples Co-authored-by: Arman Tekinalp <53585636+armantekinalp@users.noreply.github.com> Co-authored-by: Yashraj Bhosale Co-authored-by: Seung Hyun Kim --- .../generic_system_type_connection.py | 412 ++++++++++++++++++ elastica/joint.py | 37 +- .../generic_system_type_fixed_joint.py | 201 +++++++++ .../generic_system_type_spherical_joint.py | 197 +++++++++ examples/JointCases/fixed_joint.py | 54 ++- examples/JointCases/fixed_joint_torsion.py | 18 +- examples/JointCases/hinge_joint.py | 34 +- .../JointCases/joint_cases_postprocessing.py | 184 +++++++- examples/JointCases/spherical_joint.py | 70 ++- .../test_generic_system_type_joint.py | 277 ++++++++++++ tests/test_joint.py | 35 +- 11 files changed, 1438 insertions(+), 81 deletions(-) create mode 100644 elastica/experimental/connection_contact_joint/generic_system_type_connection.py create mode 100644 examples/JointCases/experimental/generic_system_type_fixed_joint.py create mode 100644 examples/JointCases/experimental/generic_system_type_spherical_joint.py create mode 100644 tests/test_experimental/test_generic_system_type_joint.py diff --git a/elastica/experimental/connection_contact_joint/generic_system_type_connection.py b/elastica/experimental/connection_contact_joint/generic_system_type_connection.py new file mode 100644 index 000000000..d4a0e2357 --- /dev/null +++ b/elastica/experimental/connection_contact_joint/generic_system_type_connection.py @@ -0,0 +1,412 @@ +__doc__ = ( + """ Module containing joint classes to connect rods and rigid bodies together. """ +) +__all__ = ["GenericSystemTypeFreeJoint", "GenericSystemTypeFixedJoint"] +from elastica.joint import FreeJoint, FixedJoint +from elastica.typing import SystemType +from elastica.utils import Tolerance, MaxDimension +import numpy as np +from typing import Optional + +# Experimental implementation for `joint` modules: #122 #149 +# - Enable the joint between `rod` and `rigid-body` +# - Allow joint to form offset from the node/COM +# - Generalized version for joint system +# Blocked by: +# - #113 +# TODO: +# - [x] Tests +# - [ ] Physical validation / theory / literature for reference +# - [ ] Optimization / Numba +# - [ ] Benchmark +# - [x] Examples + + +class GenericSystemTypeFreeJoint(FreeJoint): + """ + Constrains the relative movement between two nodes by applying restoring forces. + + Attributes + ---------- + k : float + Stiffness coefficient of the joint. + nu : float + Damping coefficient of the joint. + point_system_one : numpy.ndarray + Describes for system one in the local coordinate system the translation from the node `index_one` (for rods) + or the center of mass (for rigid bodies) to the joint. + point_system_two : numpy.ndarray + Describes for system two in the local coordinate system the translation from the node `index_two` (for rods) + or the center of mass (for rigid bodies) to the joint. + + + Examples + -------- + How to connect two Cosserat rods together using a spherical joint with a gap of 0.01 m in between. + + >>> simulator.connect(rod_one, rod_two, first_connect_idx=-1, second_connect_idx=0).using( + ... FreeJoint, + ... k=1e4, + ... nu=1, + ... point_system_one=np.array([0.0, 0.0, 0.005]), + ... point_system_two=np.array([0.0, 0.0, -0.005]), + ... ) + + How to connect the distal end of a CosseratRod with the base of a cylinder using a spherical joint. + + >>> simulator.connect(rod, cylinder, first_connect_idx=-1, second_connect_idx=0).using( + ... FreeJoint, + ... k=1e4, + ... nu=1, + ... point_system_two=np.array([0.0, 0.0, -cylinder.length / 2.]), + ... ) + + """ + + # pass the k and nu for the forces + # also the necessary systems for the joint + def __init__( + self, + k: float, + nu: float, + point_system_one: Optional[np.ndarray] = None, + point_system_two: Optional[np.ndarray] = None, + **kwargs, + ): + """ + + Parameters + ---------- + k : float + Stiffness coefficient of the joint. + nu : float + Damping coefficient of the joint. + point_system_one : Optional[numpy.ndarray] + Describes for system one in the local coordinate system the translation from the node `index_one` (for rods) + or the center of mass (for rigid bodies) to the joint. + (default = np.array([0.0, 0.0, 0.0])) + point_system_two : Optional[numpy.ndarray] + Describes for system two in the local coordinate system the translation from the node `index_two` (for rods) + or the center of mass (for rigid bodies) to the joint. + (default = np.array([0.0, 0.0, 0.0])) + """ + super().__init__(k=k, nu=nu, **kwargs) + + self.point_system_one = ( + point_system_one + if point_system_one is not None + else np.array([0.0, 0.0, 0.0]) + ) + self.point_system_two = ( + point_system_two + if point_system_two is not None + else np.array([0.0, 0.0, 0.0]) + ) + + def apply_forces( + self, + system_one: SystemType, + index_one: int, + system_two: SystemType, + index_two: int, + ): + """ + Apply joint force to the connected systems. + + Parameters + ---------- + system_one : SystemType + System two of the joint connection. + index_one : int + Index of first system for joint. + system_two : SystemType + System two of the joint connection. + index_two : int + Index of second system for joint. + + """ + # Compute the position in the inertial frame of the specified point. + # The point is defined in the local coordinate system of system one and used to attach to the joint. + position_system_one = compute_position_of_point( + system=system_one, point=self.point_system_one, index=index_one + ) + # Compute the position in the inertial frame of the specified point. + # The point is defined in the local coordinate system of system two and used to attach to the joint. + position_system_two = compute_position_of_point( + system=system_two, point=self.point_system_two, index=index_two + ) + + # Analogue to the positions, compute the velocities of the points in the inertial frames + velocity_system_one = compute_velocity_of_point( + system=system_one, point=self.point_system_one, index=index_one + ) + velocity_system_two = compute_velocity_of_point( + system=system_two, point=self.point_system_two, index=index_two + ) + + # Compute the translational deviation of the point belonging to system one + # from the point belonging to system two + distance_vector = position_system_two - position_system_one + + # Compute elastic force using a spring formulation as a linear function of the (undesired) distance between + # the two systems. + elastic_force = self.k * distance_vector + + # Compute the velocity deviation of the point belonging to system one from the point belonging to system two + relative_velocity = velocity_system_two - velocity_system_one + + # Compute damping force considering the specified damping coefficient `nu` + damping_force = self.nu * relative_velocity + + # compute contact force as addition of elastic force and damping force + contact_force = elastic_force + damping_force + + # loop over the two systems + for i, (system, index, point, system_position) in enumerate( + zip( + [system_one, system_two], + [index_one, index_two], + [self.point_system_one, self.point_system_two], + [position_system_one, position_system_two], + ) + ): + # The external force has opposite signs for the two systems: + # For system one: external_force = +contact_force + # For system two: external_force = -contact_force + external_force = (1 - 2 * i) * contact_force + + # the contact force needs to be applied at a distance from the Center of Mass (CoM) of the rigid body + # or the selected node of the Cosserat rod. + # This generates a torque, which we also need to apply to both systems. + # We first compute the vector r from the node / CoM to the joint connection point. + distance_system_point = ( + system_position - system.position_collection[..., index] + ) + # The torque is the cross product of the distance vector and the contact force: tau = r x F + external_torque = np.cross(distance_system_point, external_force) + + # Apply external forces and torques to both systems. + system.external_forces[..., index] += external_force + # the torque still needs to be rotated into the local coordinate system of the system + system.external_torques[..., index] += ( + system.director_collection[..., index] @ external_torque + ) + + def apply_torques( + self, + system_one: SystemType, + index_one: int, + system_two: SystemType, + index_two: int, + ): + """ + Apply restoring joint torques to the connected systems. + + In FreeJoint class, this routine simply passes. + + Parameters + ---------- + system_one : SystemType + System two of the joint connection. + index_one : int + Index of first system for joint. + system_two : SystemType + System two of the joint connection. + index_two : int + Index of second system for joint. + + """ + pass + + +class GenericSystemTypeFixedJoint(GenericSystemTypeFreeJoint): + """ + The fixed joint class restricts the relative movement and rotation + between two nodes and elements by applying restoring forces and torques. + + Attributes + ---------- + k : float + Stiffness coefficient of the joint. + nu : float + Damping coefficient of the joint. + kt : float + Rotational stiffness coefficient of the joint. + nut : float + Rotational damping coefficient of the joint. + point_system_one : numpy.ndarray + Describes for system one in the local coordinate system the translation from the node `index_one` (for rods) + or the center of mass (for rigid bodies) to the joint. + point_system_two : numpy.ndarray + Describes for system two in the local coordinate system the translation from the node `index_two` (for rods) + or the center of mass (for rigid bodies) to the joint. + rest_rotation_matrix : np.ndarray + 2D (3,3) array containing data with 'float' type. + Rest 3x3 rotation matrix from system one to system two at the connected elements. + Instead of aligning the directors of both systems directly, a desired rest rotational matrix labeled C_12* + is enforced. + + Examples + -------- + How to connect two Cosserat rods together using a fixed joint while aligning the tangents (e.g. local z-axis). + + >>> simulator.connect(rod_one, rod_two).using( + ... FixedJoint, + ... k=1e4, + ... nu=1, + ... ) + + How to connect a cosserat rod with the base of a cylinder using a fixed joint, where the cylinder is rotated + by 45 degrees around the y-axis. + + >>> from scipy.spatial.transform import Rotation + ... simulator.connect(rod, cylinder).using( + ... FixedJoint, + ... k=1e5, + ... nu=1e0, + ... kt=1e3, + ... nut=1e-3, + ... point_system_two=np.array([0, 0, -cylinder.length / 2]), + ... rest_rotation_matrix=Rotation.from_euler('y', np.pi / 4, degrees=False).as_matrix(), + ... ) + """ + + def __init__( + self, + k: float, + nu: float, + kt: float, + nut: float = 0.0, + point_system_one: Optional[np.ndarray] = None, + point_system_two: Optional[np.ndarray] = None, + rest_rotation_matrix: Optional[np.ndarray] = None, + **kwargs, + ): + """ + + Parameters + ---------- + k : float + Stiffness coefficient of the joint. + nu : float + Damping coefficient of the joint. + kt : float + Rotational stiffness coefficient of the joint. + nut : float + Rotational damping coefficient of the joint. (default=0.0) + point_system_one : Optional[numpy.ndarray] + Describes for system one in the local coordinate system the translation from the node `index_one` (for rods) + or the center of mass (for rigid bodies) to the joint. + (default = np.array([0.0, 0.0, 0.0])) + point_system_two : Optional[numpy.ndarray] + Describes for system two in the local coordinate system the translation from the node `index_two` (for rods) + or the center of mass (for rigid bodies) to the joint. + (default = np.array([0.0, 0.0, 0.0])) + rest_rotation_matrix : Optional[np.ndarray] + 2D (3,3) array containing data with 'float' type. + Rest 3x3 rotation matrix from system one to system two at the connected elements. + If provided, the rest rotation matrix is enforced between the two systems throughout the simulation. + If not provided, `rest_rotation_matrix` is initialized to the identity matrix, + which means that a restoring torque will be applied to align the directors of both systems directly. + (default=None) + """ + super().__init__( + k=k, + nu=nu, + point_system_one=point_system_one, + point_system_two=point_system_two, + **kwargs, + ) + + # set rotational spring and damping coefficients + self.kt = kt + self.nut = nut + + # TODO: compute the rest rotation matrix directly during initialization + # as soon as systems (e.g. `system_one` and `system_two`) and indices (e.g. `index_one` and `index_two`) + # are available in the __init__ + if rest_rotation_matrix is None: + rest_rotation_matrix = np.eye(3) + assert rest_rotation_matrix.shape == (3, 3), "Rest rotation matrix must be 3x3" + self.rest_rotation_matrix = rest_rotation_matrix + + # Use the `apply_torques` method of the `FixedJoint` class to apply restoring torques to the connected systems. + apply_torques = FixedJoint.apply_torques + + +def compute_position_of_point(system: SystemType, point: np.ndarray, index: int): + """ + Computes the position in the inertial frame of a point specified in the local frame of + the specified node of the rod. + + Parameters + ---------- + system: SystemType + System to which the point belongs. + point : np.ndarray + 1D (3,) numpy array containing 'float' data. + The point describes a position in the local frame relative to the inertial position of node + with index `index` and orientation of element with `index`. + index : int + Index of the node / element in the system. + + Returns + ------- + position : np.ndarray + 1D (3,) numpy array containing 'float' data. + Position of the point in the inertial frame. + + Examples + -------- + Compute position in inertial frame for a point (0, 0, 1) relative to the last node of the rod. + + >>> system.compute_position_of_point(np.array([0, 0, 1]), -1) + """ + position = ( + system.position_collection[..., index] + + system.director_collection[..., index].T @ point + ) + return position + + +def compute_velocity_of_point(system: SystemType, point: np.ndarray, index: int): + """ + Computes the velocity in the inertial frame of point specified in the local frame of a node / element. + + Parameters + ---------- + system: SystemType + System to which the point belongs. + point : np.ndarray + 1D (3,) numpy array containing 'float' data. + The point describes a position in the local frame relative to the inertial position of node + with index `index` and orientation of element with `index`. + index : int + Index of the node / element in the system. + + Returns + ------- + velocity : np.ndarray + 1D (3,) numpy array containing 'float' data. + Velocity of the point in the inertial frame. + + Examples + -------- + Compute velocity in inertial frame for a point (0, 0, 1) relative to the last node of the rod. + + >>> system.compute_velocity_of_point(np.array([0, 0, 1]), -1) + """ + # point rotated into the inertial frame + point_inertial_frame = system.director_collection[..., index].T @ point + + # rotate angular velocity to inertial frame + omega_inertial_frame = np.dot( + system.director_collection[..., index].T, system.omega_collection[..., index] + ) + + # apply the euler differentiation rule + velocity = system.velocity_collection[..., index] + np.cross( + omega_inertial_frame, point_inertial_frame + ) + + return velocity diff --git a/elastica/joint.py b/elastica/joint.py index 755b53bad..4a1176016 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -1,11 +1,12 @@ __doc__ = """ Module containing joint classes to connect multiple rods together. """ __all__ = ["FreeJoint", "HingeJoint", "FixedJoint", "ExternalContact", "SelfContact"] -import numpy as np -import numba -from elastica.utils import Tolerance, MaxDimension from elastica._linalg import _batch_product_k_ik_to_ik from elastica._rotations import _inv_rotate +from elastica.typing import SystemType +from elastica.utils import Tolerance, MaxDimension from math import sqrt +import numba +import numpy as np class FreeJoint: @@ -167,31 +168,27 @@ def __init__(self, k, nu, kt, normal_direction): self.kt = kt # Apply force is same as free joint - def apply_forces(self, rod_one, index_one, rod_two, index_two): - return super().apply_forces(rod_one, index_one, rod_two, index_two) + def apply_forces(self, system_one, index_one, system_two, index_two): + return super().apply_forces(system_one, index_one, system_two, index_two) - def apply_torques(self, rod_one, index_one, rod_two, index_two): - # current direction of the first element of link two - # also NOTE: - rod two is hinged at first element - link_direction = ( - rod_two.position_collection[..., index_two + 1] - - rod_two.position_collection[..., index_two] - ) + def apply_torques(self, system_one, index_one, system_two, index_two): + # current tangent direction of the `index_two` element of system two + system_two_tangent = system_two.director_collection[2, :, index_two] - # projection of the link direction onto the plane normal + # projection of the tangent of system two onto the plane normal force_direction = ( - -np.dot(link_direction, self.normal_direction) * self.normal_direction + -np.dot(system_two_tangent, self.normal_direction) * self.normal_direction ) # compute the restoring torque - torque = self.kt * np.cross(link_direction, force_direction) + torque = self.kt * np.cross(system_two_tangent, force_direction) # The opposite torque will be applied on link one - rod_one.external_torques[..., index_one] -= ( - rod_one.director_collection[..., index_one] @ torque + system_one.external_torques[..., index_one] -= ( + system_one.director_collection[..., index_one] @ torque ) - rod_two.external_torques[..., index_two] += ( - rod_two.director_collection[..., index_two] @ torque + system_two.external_torques[..., index_two] += ( + system_two.director_collection[..., index_two] @ torque ) @@ -893,7 +890,7 @@ class ExternalContact(FreeJoint): -------- How to define contact between rod and cylinder. - >>> simulator.connect(rod, cylidner).using( + >>> simulator.connect(rod, cylinder).using( ... ExternalContact, ... k=1e4, ... nu=10, diff --git a/examples/JointCases/experimental/generic_system_type_fixed_joint.py b/examples/JointCases/experimental/generic_system_type_fixed_joint.py new file mode 100644 index 000000000..551e36522 --- /dev/null +++ b/examples/JointCases/experimental/generic_system_type_fixed_joint.py @@ -0,0 +1,201 @@ +__doc__ = """Fixed joint example, for detailed explanation refer to Zhang et. al. Nature Comm. methods section.""" + +import numpy as np +from elastica import * +from elastica.experimental.connection_contact_joint.generic_system_type_connection import ( + GenericSystemTypeFixedJoint, +) +from examples.JointCases.joint_cases_postprocessing import ( + plot_position, + plot_video, + plot_video_xy, + plot_video_xz, +) + + +class FixedJointSimulator( + BaseSystemCollection, Constraints, Connections, Forcing, Damping, CallBacks +): + pass + + +fixed_joint_sim = FixedJointSimulator() + +# setting up test params +n_elem = 10 +direction = np.array([0.0, 0.0, 1.0]) +normal = np.array([0.0, 1.0, 0.0]) +roll_direction = np.cross(direction, normal) +base_length = 0.2 +base_radius = 0.007 +base_area = np.pi * base_radius ** 2 +density = 1750 +E = 3e7 +poisson_ratio = 0.5 +shear_modulus = E / (poisson_ratio + 1.0) + +# setting up time params +final_time = 10 +dt = 5e-5 +fps = 100 # fps of the video +step_skip = int(1 / (dt * fps)) + +start_rod_1 = np.zeros((3,)) +start_rod_2 = start_rod_1 + direction * base_length +start_cylinder = start_rod_2 + direction * base_length + +# Create rod 1 +rod1 = CosseratRod.straight_rod( + n_elem, + start_rod_1, + direction, + normal, + base_length, + base_radius, + density, + 0.0, # internal damping constant, deprecated in v0.3.0 + E, + shear_modulus=shear_modulus, +) +fixed_joint_sim.append(rod1) +# Create rod 2 +rod2 = CosseratRod.straight_rod( + n_elem, + start_rod_2, + direction, + normal, + base_length, + base_radius, + density, + 0.0, # internal damping constant, deprecated in v0.3.0 + E, + shear_modulus=shear_modulus, +) +fixed_joint_sim.append(rod2) +cylinder = Cylinder( + start=start_cylinder, + direction=direction, + normal=normal, + base_length=base_length, + base_radius=base_radius, + density=density, +) +fixed_joint_sim.append(cylinder) + +# Apply boundary conditions to rod1. +fixed_joint_sim.constrain(rod1).using( + OneEndFixedBC, constrained_position_idx=(0,), constrained_director_idx=(0,) +) + +# Connect rod 1 and rod 2 +fixed_joint_sim.connect( + first_rod=rod1, second_rod=rod2, first_connect_idx=-1, second_connect_idx=0 +).using( + GenericSystemTypeFixedJoint, + k=1e5, + nu=0e0, + kt=1e1, + nut=0e0, +) +# Connect rod 2 and cylinder +fixed_joint_sim.connect( + first_rod=rod2, second_rod=cylinder, first_connect_idx=-1, second_connect_idx=0 +).using( + GenericSystemTypeFixedJoint, + k=1e5, + nu=0e0, + kt=1e1, + nut=0e0, + point_system_two=np.array([0.0, 0.0, -cylinder.length / 2]), +) + +# Add forces to rod2 +fixed_joint_sim.add_forcing_to(rod2).using( + EndpointForcesSinusoidal, + start_force_mag=0, + end_force_mag=5e-3, + ramp_up_time=0.2, + tangent_direction=direction, + normal_direction=normal, +) + +# add damping +# old damping model (deprecated in v0.3.0) values +# damping_constant = 0.4 +# dt = 1e-5 +damping_constant = 0.4 +fixed_joint_sim.dampen(rod1).using( + AnalyticalLinearDamper, + damping_constant=damping_constant, + time_step=dt, +) +fixed_joint_sim.dampen(rod2).using( + AnalyticalLinearDamper, + damping_constant=damping_constant, + time_step=dt, +) + +pp_list_rod1 = defaultdict(list) +pp_list_rod2 = defaultdict(list) +pp_list_cylinder = defaultdict(list) + +fixed_joint_sim.collect_diagnostics(rod1).using( + MyCallBack, step_skip=step_skip, callback_params=pp_list_rod1 +) +fixed_joint_sim.collect_diagnostics(rod2).using( + MyCallBack, step_skip=step_skip, callback_params=pp_list_rod2 +) +fixed_joint_sim.collect_diagnostics(cylinder).using( + MyCallBack, step_skip=step_skip, callback_params=pp_list_cylinder +) + +fixed_joint_sim.finalize() +timestepper = PositionVerlet() +# timestepper = PEFRL() + +dl = base_length / n_elem +total_steps = int(final_time / dt) +print("Total steps", total_steps) +integrate(timestepper, fixed_joint_sim, final_time, total_steps) + +PLOT_FIGURE = True +SAVE_FIGURE = True +PLOT_VIDEO = True + +# plotting results +if PLOT_FIGURE: + filename = "generic_system_type_fixed_joint_example_last_node_pos_xy.png" + plot_position( + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=pp_list_cylinder, + filename=filename, + SAVE_FIGURE=SAVE_FIGURE, + ) + +if PLOT_VIDEO: + filename = "generic_system_type_fixed_joint_example" + plot_video( + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=pp_list_cylinder, + cylinder=cylinder, + video_name=filename + ".mp4", + fps=fps, + ) + plot_video_xy( + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=pp_list_cylinder, + cylinder=cylinder, + video_name=filename + "_xy.mp4", + fps=fps, + ) + plot_video_xz( + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=pp_list_cylinder, + cylinder=cylinder, + video_name=filename + "_xz.mp4", + fps=fps, + ) diff --git a/examples/JointCases/experimental/generic_system_type_spherical_joint.py b/examples/JointCases/experimental/generic_system_type_spherical_joint.py new file mode 100644 index 000000000..7d447b5d3 --- /dev/null +++ b/examples/JointCases/experimental/generic_system_type_spherical_joint.py @@ -0,0 +1,197 @@ +__doc__ = """Spherical(Free) joint example, for detailed explanation refer to Zhang et. al. Nature Comm. +methods section.""" + +import numpy as np +from elastica import * +from elastica.experimental.connection_contact_joint.generic_system_type_connection import ( + GenericSystemTypeFreeJoint, +) +from examples.JointCases.joint_cases_postprocessing import ( + plot_position, + plot_video, + plot_video_xy, + plot_video_xz, +) + + +class SphericalJointSimulator( + BaseSystemCollection, Constraints, Connections, Forcing, Damping, CallBacks +): + pass + + +spherical_joint_sim = SphericalJointSimulator() + +# setting up test params +n_elem = 10 +direction = np.array([0.0, 0.0, 1.0]) +normal = np.array([0.0, 1.0, 0.0]) +roll_direction = np.cross(direction, normal) +base_length = 0.2 +base_radius = 0.007 +base_area = np.pi * base_radius ** 2 +density = 1750 +E = 3e7 +poisson_ratio = 0.5 +shear_modulus = E / (poisson_ratio + 1.0) + +# setting up time params +final_time = 10 +dt = 5e-5 +fps = 100 # fps of the video +step_skip = int(1 / (dt * fps)) + +start_rod_1 = np.zeros((3,)) +start_rod_2 = start_rod_1 + direction * base_length +start_cylinder = start_rod_2 + direction * base_length + +# Create rod 1 +rod1 = CosseratRod.straight_rod( + n_elem, + start_rod_1, + direction, + normal, + base_length, + base_radius, + density, + 0.0, # internal damping constant, deprecated in v0.3.0 + E, + shear_modulus=shear_modulus, +) +spherical_joint_sim.append(rod1) +# Create rod 2 +rod2 = CosseratRod.straight_rod( + n_elem, + start_rod_2, + direction, + normal, + base_length, + base_radius, + density, + 0.0, # internal damping constant, deprecated in v0.3.0 + E, + shear_modulus=shear_modulus, +) +spherical_joint_sim.append(rod2) +# Create cylinder +cylinder = Cylinder( + start=start_cylinder, + direction=direction, + normal=normal, + base_length=base_length, + base_radius=base_radius, + density=density, +) +spherical_joint_sim.append(cylinder) + +# Apply boundary conditions to rod1. +spherical_joint_sim.constrain(rod1).using( + OneEndFixedBC, constrained_position_idx=(0,), constrained_director_idx=(0,) +) + +# Connect rod 1 and rod 2 +spherical_joint_sim.connect( + first_rod=rod1, second_rod=rod2, first_connect_idx=-1, second_connect_idx=0 +).using( + GenericSystemTypeFreeJoint, k=1e5, nu=0 +) # k=kg/s2 nu=kg/s 1e-2 +# Connect rod 2 and cylinder +spherical_joint_sim.connect( + first_rod=rod2, second_rod=cylinder, first_connect_idx=-1, second_connect_idx=0 +).using( + GenericSystemTypeFreeJoint, + k=1e5, + nu=0, + point_system_two=np.array([0.0, 0.0, -cylinder.length / 2]), +) + +# Add forces to rod2 +spherical_joint_sim.add_forcing_to(rod2).using( + EndpointForcesSinusoidal, + start_force_mag=0, + end_force_mag=5e-3, + ramp_up_time=0.2, + tangent_direction=direction, + normal_direction=normal, +) + +# add damping +# old damping model (deprecated in v0.3.0) values +# damping_constant = 4e-3 +# dt = 1e-5 +damping_constant = 4e-3 +spherical_joint_sim.dampen(rod1).using( + AnalyticalLinearDamper, + damping_constant=damping_constant, + time_step=dt, +) +spherical_joint_sim.dampen(rod2).using( + AnalyticalLinearDamper, + damping_constant=damping_constant, + time_step=dt, +) + +pp_list_rod1 = defaultdict(list) +pp_list_rod2 = defaultdict(list) +pp_list_cylinder = defaultdict(list) + +spherical_joint_sim.collect_diagnostics(rod1).using( + MyCallBack, step_skip=step_skip, callback_params=pp_list_rod1 +) +spherical_joint_sim.collect_diagnostics(rod2).using( + MyCallBack, step_skip=step_skip, callback_params=pp_list_rod2 +) +spherical_joint_sim.collect_diagnostics(cylinder).using( + MyCallBack, step_skip=step_skip, callback_params=pp_list_cylinder +) + +spherical_joint_sim.finalize() +timestepper = PositionVerlet() +# timestepper = PEFRL() + +dl = base_length / n_elem +total_steps = int(final_time / dt) +print("Total steps", total_steps) +integrate(timestepper, spherical_joint_sim, final_time, total_steps) + +PLOT_FIGURE = True +SAVE_FIGURE = True +PLOT_VIDEO = True + +# plotting results +if PLOT_FIGURE: + filename = "generic_system_type_spherical_joint_example_last_node_pos_xy.png" + plot_position( + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=pp_list_cylinder, + filename=filename, + SAVE_FIGURE=SAVE_FIGURE, + ) + +if PLOT_VIDEO: + filename = "generic_system_type_spherical_joint_example" + plot_video( + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=pp_list_cylinder, + cylinder=cylinder, + video_name=filename + ".mp4", + fps=fps, + ) + plot_video_xy( + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=pp_list_cylinder, + cylinder=cylinder, + video_name=filename + "_xy.mp4", + fps=fps, + ) + plot_video_xz( + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=pp_list_cylinder, + cylinder=cylinder, + video_name=filename + "_xz.mp4", + fps=fps, + ) diff --git a/examples/JointCases/fixed_joint.py b/examples/JointCases/fixed_joint.py index 0f6b63e95..46a7e66ed 100644 --- a/examples/JointCases/fixed_joint.py +++ b/examples/JointCases/fixed_joint.py @@ -31,6 +31,12 @@ class FixedJointSimulator( poisson_ratio = 0.5 shear_modulus = E / (poisson_ratio + 1.0) +# setting up time params +final_time = 10 +dt = 5e-5 +fps = 100 # fps of the video +step_skip = int(1 / (dt * fps)) + start_rod_1 = np.zeros((3,)) start_rod_2 = start_rod_1 + direction * base_length @@ -71,7 +77,13 @@ class FixedJointSimulator( # Connect rod 1 and rod 2 fixed_joint_sim.connect( first_rod=rod1, second_rod=rod2, first_connect_idx=-1, second_connect_idx=0 -).using(FixedJoint, k=1e5, nu=0, kt=5e3) +).using( + FixedJoint, + k=1e5, + nu=0e0, + kt=1e1, + nut=0e0, +) # Add forces to rod2 fixed_joint_sim.add_forcing_to(rod2).using( @@ -88,7 +100,6 @@ class FixedJointSimulator( # damping_constant = 0.4 # dt = 1e-5 damping_constant = 0.4 -dt = 1e-4 fixed_joint_sim.dampen(rod1).using( AnalyticalLinearDamper, damping_constant=damping_constant, @@ -104,37 +115,56 @@ class FixedJointSimulator( pp_list_rod2 = defaultdict(list) fixed_joint_sim.collect_diagnostics(rod1).using( - MyCallBack, step_skip=1000, callback_params=pp_list_rod1 + MyCallBack, step_skip=step_skip, callback_params=pp_list_rod1 ) fixed_joint_sim.collect_diagnostics(rod2).using( - MyCallBack, step_skip=1000, callback_params=pp_list_rod2 + MyCallBack, step_skip=step_skip, callback_params=pp_list_rod2 ) fixed_joint_sim.finalize() timestepper = PositionVerlet() # timestepper = PEFRL() -final_time = 10 dl = base_length / n_elem total_steps = int(final_time / dt) print("Total steps", total_steps) integrate(timestepper, fixed_joint_sim, final_time, total_steps) PLOT_FIGURE = True -SAVE_FIGURE = False +SAVE_FIGURE = True PLOT_VIDEO = True # plotting results if PLOT_FIGURE: - filename = "fixed_joint_test.png" - plot_position(pp_list_rod1, pp_list_rod2, filename, SAVE_FIGURE) + filename = "fixed_joint_example_last_node_pos_xy.png" + plot_position( + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=None, + filename=filename, + SAVE_FIGURE=SAVE_FIGURE, + ) if PLOT_VIDEO: - filename = "fixed_joint_test.mp4" - plot_video(pp_list_rod1, pp_list_rod2, video_name=filename, margin=0.2, fps=100) + filename = "fixed_joint_example" + plot_video( + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=None, + video_name=filename + ".mp4", + fps=fps, + ) plot_video_xy( - pp_list_rod1, pp_list_rod2, video_name=filename + "_xy.mp4", margin=0.2, fps=100 + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=None, + video_name=filename + "_xy.mp4", + fps=fps, ) plot_video_xz( - pp_list_rod1, pp_list_rod2, video_name=filename + "_xz.mp4", margin=0.2, fps=100 + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=None, + video_name=filename + "_xz.mp4", + fps=fps, ) diff --git a/examples/JointCases/fixed_joint_torsion.py b/examples/JointCases/fixed_joint_torsion.py index 27a281a9f..bd08f9ba9 100644 --- a/examples/JointCases/fixed_joint_torsion.py +++ b/examples/JointCases/fixed_joint_torsion.py @@ -142,30 +142,36 @@ class FixedJointSimulator( # plotting results if PLOT_FIGURE: - filename = "fixed_joint_torsion_test.png" - plot_position(pp_list_rod1, pp_list_rod2, filename, SAVE_FIGURE) + filename = "fixed_joint_torsion_example_last_node_pos_xy.png" + plot_position( + pp_list_rod1, + pp_list_rod2, + plot_params_cylinder=None, + filename=filename, + SAVE_FIGURE=SAVE_FIGURE, + ) if PLOT_VIDEO: - filename = "fixed_joint_torsion_test" + filename = "fixed_joint_torsion_example" fps = 100 # Hz plot_video( pp_list_rod1, pp_list_rod2, + plot_params_cylinder=None, video_name=filename + ".mp4", - margin=0.2, fps=fps, ) plot_video_xy( pp_list_rod1, pp_list_rod2, + plot_params_cylinder=None, video_name=filename + "_xy.mp4", - margin=0.2, fps=fps, ) plot_video_xz( pp_list_rod1, pp_list_rod2, + plot_params_cylinder=None, video_name=filename + "_xz.mp4", - margin=0.2, fps=fps, ) diff --git a/examples/JointCases/hinge_joint.py b/examples/JointCases/hinge_joint.py index 7f9a9b995..226394ec3 100644 --- a/examples/JointCases/hinge_joint.py +++ b/examples/JointCases/hinge_joint.py @@ -72,7 +72,7 @@ class HingeJointSimulator( hinge_joint_sim.connect( first_rod=rod1, second_rod=rod2, first_connect_idx=-1, second_connect_idx=0 ).using( - HingeJoint, k=1e5, nu=0, kt=5e3, normal_direction=roll_direction + HingeJoint, k=1e5, nu=0, kt=1e1, normal_direction=roll_direction ) # 1e-2 # Add forces to rod2 @@ -128,15 +128,35 @@ class HingeJointSimulator( # plotting results if PLOT_FIGURE: - filename = "hinge_joint_test.png" - plot_position(pp_list_rod1, pp_list_rod2, filename, SAVE_FIGURE) + filename = "hinge_joint_example_last_node_pos_xy.png" + plot_position( + pp_list_rod1, + pp_list_rod2, + plot_params_cylinder=None, + filename=filename, + SAVE_FIGURE=SAVE_FIGURE, + ) if PLOT_VIDEO: - filename = "hinge_joint_test.mp4" - plot_video(pp_list_rod1, pp_list_rod2, video_name=filename, margin=0.2, fps=100) + filename = "hinge_joint_example" + plot_video( + pp_list_rod1, + pp_list_rod2, + plot_params_cylinder=None, + video_name=filename + ".mp4", + fps=100, + ) plot_video_xy( - pp_list_rod1, pp_list_rod2, video_name=filename + "_xy.mp4", margin=0.2, fps=100 + pp_list_rod1, + pp_list_rod2, + plot_params_cylinder=None, + video_name=filename + "_xy.mp4", + fps=100, ) plot_video_xz( - pp_list_rod1, pp_list_rod2, video_name=filename + "_xz.mp4", margin=0.2, fps=100 + pp_list_rod1, + pp_list_rod2, + plot_params_cylinder=None, + video_name=filename + "_xz.mp4", + fps=100, ) diff --git a/examples/JointCases/joint_cases_postprocessing.py b/examples/JointCases/joint_cases_postprocessing.py index d7ea14376..c1ca4dd46 100644 --- a/examples/JointCases/joint_cases_postprocessing.py +++ b/examples/JointCases/joint_cases_postprocessing.py @@ -4,16 +4,24 @@ from mpl_toolkits import mplot3d from scipy.spatial.transform import Rotation +from elastica.rigidbody import Cylinder +from elastica._linalg import _batch_matvec + def plot_position( plot_params_rod1: dict, plot_params_rod2: dict, - filename="spherical_joint_test.png", + plot_params_cylinder: dict = None, + filename="joint_cases_last_node_pos_xy.png", SAVE_FIGURE=False, ): - position_of_rod1 = np.array(plot_params_rod1["position"]) position_of_rod2 = np.array(plot_params_rod2["position"]) + position_of_cylinder = ( + np.array(plot_params_cylinder["position"]) + if plot_params_cylinder is not None + else None + ) fig = plt.figure(figsize=(10, 10), frameon=True, dpi=150) ax = fig.add_subplot(111) @@ -27,8 +35,17 @@ def plot_position( c=to_rgb("xkcd:bluish"), label="rod2", ) + if position_of_cylinder is not None: + ax.plot( + position_of_cylinder[:, 0, -1], + position_of_cylinder[:, 1, -1], + c=to_rgb("xkcd:greenish"), + label="cylinder", + ) fig.legend(prop={"size": 20}) + plt.xlabel("x") + plt.ylabel("y") plt.show() @@ -37,6 +54,9 @@ def plot_position( def plot_orientation(title, time, directors): + """ + Plot the orientation of one node + """ quat = [] for t in range(len(time)): quat_t = Rotation.from_matrix(directors[t].T).as_quat() @@ -58,15 +78,26 @@ def plot_orientation(title, time, directors): def plot_video( plot_params_rod1: dict, plot_params_rod2: dict, - video_name="video.mp4", - margin=0.2, - fps=15, + plot_params_cylinder: dict = None, + video_name="joint_cases_video.mp4", + fps=100, + cylinder: Cylinder = None, ): # (time step, x/y/z, node) import matplotlib.animation as manimation time = plot_params_rod1["time"] position_of_rod1 = np.array(plot_params_rod1["position"]) position_of_rod2 = np.array(plot_params_rod2["position"]) + position_of_cylinder = ( + np.array(plot_params_cylinder["position"]) + if plot_params_cylinder is not None + else None + ) + director_of_cylinder = ( + np.array(plot_params_cylinder["directors"]) + if plot_params_cylinder is not None + else None + ) print("plot video") FFMpegWriter = manimation.writers["ffmpeg"] @@ -94,25 +125,73 @@ def plot_video( c=to_rgb("xkcd:bluish"), label="rod2", ) + if position_of_cylinder is not None: + ax.plot( + position_of_cylinder[time, 0], + position_of_cylinder[time, 1], + position_of_cylinder[time, 2], + "o", + c=to_rgb("xkcd:greenish"), + label="Cylinder CoM", + ) + if cylinder is not None: + cylinder_axis_points = np.zeros((3, 10)) + cylinder_axis_points[2, :] = np.linspace( + start=-cylinder.length.squeeze() / 2, + stop=cylinder.length.squeeze() / 2, + num=cylinder_axis_points.shape[1], + ) + cylinder_director = director_of_cylinder[time, ...] + cylinder_director_batched = np.repeat( + cylinder_director, repeats=cylinder_axis_points.shape[1], axis=2 + ) + # rotate points into inertial frame + cylinder_axis_points = _batch_matvec( + cylinder_director_batched.transpose((1, 0, 2)), + cylinder_axis_points, + ) + # add offset position of CoM + cylinder_axis_points += position_of_cylinder[time, ...] + ax.plot( + cylinder_axis_points[0, :], + cylinder_axis_points[1, :], + cylinder_axis_points[2, :], + c=to_rgb("xkcd:greenish"), + label="Cylinder axis", + ) ax.set_xlim(-0.25, 0.25) ax.set_ylim(-0.25, 0.25) - ax.set_zlim(0, 0.4) + ax.set_zlim(0, 0.61) + ax.set_xlabel("x [m]") + ax.set_ylabel("y [m]") + ax.set_zlabel("z [m]") writer.grab_frame() def plot_video_xy( plot_params_rod1: dict, plot_params_rod2: dict, - video_name="video.mp4", - margin=0.2, - fps=15, + plot_params_cylinder: dict = None, + video_name="joint_cases_video_xy.mp4", + fps=100, + cylinder: Cylinder = None, ): # (time step, x/y/z, node) import matplotlib.animation as manimation time = plot_params_rod1["time"] position_of_rod1 = np.array(plot_params_rod1["position"]) position_of_rod2 = np.array(plot_params_rod2["position"]) + position_of_cylinder = ( + np.array(plot_params_cylinder["position"]) + if plot_params_cylinder is not None + else None + ) + director_of_cylinder = ( + np.array(plot_params_cylinder["directors"]) + if plot_params_cylinder is not None + else None + ) print("plot video xy") FFMpegWriter = manimation.writers["ffmpeg"] @@ -133,24 +212,69 @@ def plot_video_xy( c=to_rgb("xkcd:bluish"), label="rod2", ) + if position_of_cylinder is not None: + plt.plot( + position_of_cylinder[time, 0], + position_of_cylinder[time, 1], + "o", + c=to_rgb("xkcd:greenish"), + label="cylinder", + ) + if cylinder is not None: + cylinder_axis_points = np.zeros((3, 10)) + cylinder_axis_points[2, :] = np.linspace( + start=-cylinder.length.squeeze() / 2, + stop=cylinder.length.squeeze() / 2, + num=cylinder_axis_points.shape[1], + ) + cylinder_director = director_of_cylinder[time, ...] + cylinder_director_batched = np.repeat( + cylinder_director, repeats=cylinder_axis_points.shape[1], axis=2 + ) + # rotate points into inertial frame + cylinder_axis_points = _batch_matvec( + cylinder_director_batched.transpose((1, 0, 2)), + cylinder_axis_points, + ) + # add offset position of CoM + cylinder_axis_points += position_of_cylinder[time, ...] + plt.plot( + cylinder_axis_points[0, :], + cylinder_axis_points[1, :], + c=to_rgb("xkcd:greenish"), + label="Cylinder axis", + ) plt.xlim([-0.25, 0.25]) plt.ylim([-0.25, 0.25]) + plt.xlabel("x [m]") + plt.ylabel("y [m]") writer.grab_frame() def plot_video_xz( plot_params_rod1: dict, plot_params_rod2: dict, - video_name="video.mp4", - margin=0.2, - fps=15, + plot_params_cylinder: dict = None, + video_name="joint_cases_video_xz.mp4", + fps=100, + cylinder: Cylinder = None, ): # (time step, x/y/z, node) import matplotlib.animation as manimation time = plot_params_rod1["time"] position_of_rod1 = np.array(plot_params_rod1["position"]) position_of_rod2 = np.array(plot_params_rod2["position"]) + position_of_cylinder = ( + np.array(plot_params_cylinder["position"]) + if plot_params_cylinder is not None + else None + ) + director_of_cylinder = ( + np.array(plot_params_cylinder["directors"]) + if plot_params_cylinder is not None + else None + ) print("plot video xz") FFMpegWriter = manimation.writers["ffmpeg"] @@ -171,7 +295,41 @@ def plot_video_xz( c=to_rgb("xkcd:bluish"), label="rod2", ) + if position_of_cylinder is not None: + plt.plot( + position_of_cylinder[time, 0], + position_of_cylinder[time, 2], + "o", + c=to_rgb("xkcd:greenish"), + label="cylinder", + ) + if cylinder is not None: + cylinder_axis_points = np.zeros((3, 10)) + cylinder_axis_points[2, :] = np.linspace( + start=-cylinder.length.squeeze() / 2, + stop=cylinder.length.squeeze() / 2, + num=cylinder_axis_points.shape[1], + ) + cylinder_director = director_of_cylinder[time, ...] + cylinder_director_batched = np.repeat( + cylinder_director, repeats=cylinder_axis_points.shape[1], axis=2 + ) + # rotate points into inertial frame + cylinder_axis_points = _batch_matvec( + cylinder_director_batched.transpose((1, 0, 2)), + cylinder_axis_points, + ) + # add offset position of CoM + cylinder_axis_points += position_of_cylinder[time, ...] + plt.plot( + cylinder_axis_points[0, :], + cylinder_axis_points[2, :], + c=to_rgb("xkcd:greenish"), + label="Cylinder axis", + ) plt.xlim([-0.25, 0.25]) - plt.ylim([0, 0.41]) + plt.ylim([0, 0.61]) + plt.xlabel("x [m]") + plt.ylabel("z [m]") writer.grab_frame() diff --git a/examples/JointCases/spherical_joint.py b/examples/JointCases/spherical_joint.py index b7c7a6b89..7c73b7fd1 100644 --- a/examples/JointCases/spherical_joint.py +++ b/examples/JointCases/spherical_joint.py @@ -32,8 +32,15 @@ class SphericalJointSimulator( poisson_ratio = 0.5 shear_modulus = E / (poisson_ratio + 1.0) +# setting up time params +final_time = 10 +dt = 5e-5 +fps = 100 # fps of the video +step_skip = int(1 / (dt * fps)) + start_rod_1 = np.zeros((3,)) start_rod_2 = start_rod_1 + direction * base_length +start_cylinder = start_rod_2 + direction * base_length # Create rod 1 rod1 = CosseratRod.straight_rod( @@ -63,6 +70,16 @@ class SphericalJointSimulator( shear_modulus=shear_modulus, ) spherical_joint_sim.append(rod2) +# Create cylinder +cylinder = Cylinder( + start=start_cylinder, + direction=direction, + normal=normal, + base_length=base_length, + base_radius=base_radius, + density=density, +) +spherical_joint_sim.append(cylinder) # Apply boundary conditions to rod1. spherical_joint_sim.constrain(rod1).using( @@ -75,6 +92,12 @@ class SphericalJointSimulator( ).using( FreeJoint, k=1e5, nu=0 ) # k=kg/s2 nu=kg/s 1e-2 +# Connect rod 2 and cylinder +spherical_joint_sim.connect( + first_rod=rod2, second_rod=cylinder, first_connect_idx=-1, second_connect_idx=0 +).using( + FreeJoint, k=1e5, nu=0, point_system_two=np.array([0.0, 0.0, -cylinder.length / 2]) +) # Add forces to rod2 spherical_joint_sim.add_forcing_to(rod2).using( @@ -91,7 +114,6 @@ class SphericalJointSimulator( # damping_constant = 4e-3 # dt = 1e-5 damping_constant = 4e-3 -dt = 5e-5 spherical_joint_sim.dampen(rod1).using( AnalyticalLinearDamper, damping_constant=damping_constant, @@ -105,39 +127,65 @@ class SphericalJointSimulator( pp_list_rod1 = defaultdict(list) pp_list_rod2 = defaultdict(list) +pp_list_cylinder = defaultdict(list) spherical_joint_sim.collect_diagnostics(rod1).using( - MyCallBack, step_skip=1000, callback_params=pp_list_rod1 + MyCallBack, step_skip=step_skip, callback_params=pp_list_rod1 ) spherical_joint_sim.collect_diagnostics(rod2).using( - MyCallBack, step_skip=1000, callback_params=pp_list_rod2 + MyCallBack, step_skip=step_skip, callback_params=pp_list_rod2 +) +spherical_joint_sim.collect_diagnostics(cylinder).using( + MyCallBack, step_skip=step_skip, callback_params=pp_list_cylinder ) spherical_joint_sim.finalize() timestepper = PositionVerlet() # timestepper = PEFRL() -final_time = 10 dl = base_length / n_elem total_steps = int(final_time / dt) print("Total steps", total_steps) integrate(timestepper, spherical_joint_sim, final_time, total_steps) PLOT_FIGURE = True -SAVE_FIGURE = False +SAVE_FIGURE = True PLOT_VIDEO = True # plotting results if PLOT_FIGURE: - filename = "spherical_joint_test_last_node_pos_xy.png" - plot_position(pp_list_rod1, pp_list_rod2, filename, SAVE_FIGURE) + filename = "spherical_joint_example_last_node_pos_xy.png" + plot_position( + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=pp_list_cylinder, + filename=filename, + SAVE_FIGURE=SAVE_FIGURE, + ) if PLOT_VIDEO: - filename = "spherical_joint_test.mp4" - plot_video(pp_list_rod1, pp_list_rod2, video_name=filename, margin=0.2, fps=100) + filename = "spherical_joint_example" + plot_video( + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=pp_list_cylinder, + cylinder=cylinder, + video_name=filename + ".mp4", + fps=fps, + ) plot_video_xy( - pp_list_rod1, pp_list_rod2, video_name=filename + "_xy.mp4", margin=0.2, fps=100 + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=pp_list_cylinder, + cylinder=cylinder, + video_name=filename + "_xy.mp4", + fps=fps, ) plot_video_xz( - pp_list_rod1, pp_list_rod2, video_name=filename + "_xz.mp4", margin=0.2, fps=100 + plot_params_rod1=pp_list_rod1, + plot_params_rod2=pp_list_rod2, + plot_params_cylinder=pp_list_cylinder, + cylinder=cylinder, + video_name=filename + "_xz.mp4", + fps=fps, ) diff --git a/tests/test_experimental/test_generic_system_type_joint.py b/tests/test_experimental/test_generic_system_type_joint.py new file mode 100644 index 000000000..79a8797e6 --- /dev/null +++ b/tests/test_experimental/test_generic_system_type_joint.py @@ -0,0 +1,277 @@ +__doc__ = """ Joints for generic system types test module """ + +# System imports +from elastica.experimental.connection_contact_joint.generic_system_type_connection import ( + GenericSystemTypeFreeJoint, + compute_position_of_point, + compute_velocity_of_point, +) +from numpy.testing import assert_allclose +from elastica.rigidbody import Cylinder, Sphere, RigidBodyBase +from elastica.rod.cosserat_rod import CosseratRod +from elastica.utils import Tolerance +import numpy as np +import pytest +from typing import * + +# seed random number generator +rng = np.random.default_rng(0) + + +def init_system(system_class, origin: np.array = np.array([0.0, 0.0, 0.0])): + # Some rod properties. We need them for constructor, they are not used. + n = 4 # Number of elements + normal = np.array([1.0, 0.0, 0.0]) + direction = np.array([0.0, 0.0, 1.0]) + base_length = 1 + base_radius = 0.2 + density = 1 + nu = 0.1 + + # Youngs Modulus [Pa] + youngs_modulus = 1e6 + # poisson ratio + poisson_ratio = 0.5 + shear_modulus = youngs_modulus / (poisson_ratio + 1.0) + + if system_class == CosseratRod: + system = CosseratRod.straight_rod( + n, + origin, + direction, + normal, + base_length, + base_radius, + density, + nu, + youngs_modulus, + shear_modulus=shear_modulus, + ) + elif system_class == Cylinder: + system = Cylinder( + start=(origin - base_length / 2 * direction), + direction=direction, + normal=normal, + base_length=base_length, + base_radius=base_radius, + density=density, + ) + elif system_class == Sphere: + system = Sphere(origin, base_radius, density) + else: + raise ValueError + + return system + + +@pytest.mark.parametrize("system_one_class", [CosseratRod, Sphere, Cylinder]) +@pytest.mark.parametrize("system_two_class", [CosseratRod, Sphere, Cylinder]) +@pytest.mark.parametrize( + "point_system_one", + [ + np.zeros((3,)), + np.array([0.0, 0.0, 1.0]), + rng.random(3), + rng.random(3), + ], +) +@pytest.mark.parametrize( + "point_system_two", + [ + np.zeros((3,)), + np.array([0.0, 0.0, -1.0]), + rng.random(3), + rng.random(3), + ], +) +def test_generic_free_joint( + system_one_class: Union[Type[CosseratRod], Type[RigidBodyBase]], + system_two_class: Union[Type[CosseratRod], Type[RigidBodyBase]], + point_system_one: np.ndarray, + point_system_two: np.ndarray, +): + + # Origin of the systems + origin1 = np.array([0.0, 0.0, 0.0]) + origin2 = np.array([1.0, 0.0, 0.0]) + + system_one = init_system(system_one_class, origin1) + system_two = init_system(system_one_class, origin2) + + # Stiffness between points + k = 1e8 + + # Damping between two points + nu = 1 + + # System indices + system_one_index = -1 + system_two_index = 0 + + # System velocity + v1 = np.array([-1, 0, 0]).reshape(3, 1) + v2 = v1 * -1 + system_one.velocity_collection = np.tile( + v1, (1, system_one.velocity_collection.shape[1] + 1) + ) + system_two.velocity_collection = np.tile( + v2, (1, system_two.velocity_collection.shape[1] + 1) + ) + + # System angular velocity + omega1 = np.array([0.0, 1.0, 0.0]).reshape(3, 1) + omega2 = -omega1 + system_one.omega_collection = np.tile( + omega1, (1, system_one.omega_collection.shape[1] + 1) + ) + system_two.omega_collection = np.tile( + omega2, (1, system_two.omega_collection.shape[1] + 1) + ) + + # Verify positions + position_system_one = compute_position_of_point( + system=system_one, point=point_system_one, index=system_one_index + ) + position_system_two = compute_position_of_point( + system=system_two, point=point_system_two, index=system_two_index + ) + + # this is possible because neither system is rotated yet and still in the initial position + # and because the rotation between inertial frame and local frame of the bodies is zero at the start. + # The second point can be achieved by choosing the "normal" and "direction" vectors smartly. + assert_allclose( + position_system_one, + system_one.position_collection[:, system_one_index] + point_system_one, + ) + assert_allclose( + position_system_two, + system_two.position_collection[:, system_two_index] + point_system_two, + ) + + # Compute the distance between the connection points + distance = position_system_two - position_system_one + end_distance = np.linalg.norm(distance) + if end_distance <= Tolerance.atol(): + end_distance = 1.0 + + # Verify velocities calculated for system one + velocity_system_one = compute_velocity_of_point( + system=system_one, point=point_system_one, index=system_one_index + ) + if np.array_equal(point_system_one, np.zeros((3, 1))): + # for the joint on the node / CoM, + # the velocity of the joint needs to be the same as the velocity of the node of system one + assert_allclose( + velocity_system_one, + system_one.velocity_collection[..., system_one_index], + atol=Tolerance.atol(), + ) + elif np.array_equal(point_system_one, np.array([0.0, 0.0, 1.0])): + # analytical computation of velocity for special case of point_system_two == np.array([0.0, 0.0, 1.0]) + assert_allclose( + velocity_system_one, + system_one.velocity_collection[..., system_one_index] + + np.array([1.0, 0.0, 0.0]), + atol=Tolerance.atol(), + ) + + # Verify velocities calculated for system two + velocity_system_two = compute_velocity_of_point( + system=system_two, point=point_system_two, index=system_two_index + ) + if np.array_equal(point_system_two, np.zeros((3, 1))): + # for the joint on the node / CoM, + # the velocity of the joint needs to be the same as the velocity of the node of system two + assert_allclose( + velocity_system_two, + system_two.velocity_collection[..., system_two_index], + atol=Tolerance.atol(), + ) + elif np.array_equal(point_system_two, np.array([0.0, 0.0, -1.0])): + # analytical computation of velocity for special case of point_system_two == np.array([0.0, 0.0, -1.0]) + assert_allclose( + velocity_system_two, + system_two.velocity_collection[..., system_two_index] + + np.array([1.0, 0.0, 0.0]), + atol=Tolerance.atol(), + ) + + # Compute the relative velocity + relative_vel = velocity_system_two - velocity_system_one + + # Compute the free joint forces + elastic_force = k * distance + damping_force = nu * relative_vel + external_force = elastic_force + damping_force + + external_force_system_one = external_force + external_force_system_two = -1 * external_force + + frjt = GenericSystemTypeFreeJoint( + k=k, + nu=nu, + point_system_one=point_system_one, + point_system_two=point_system_two, + ) + frjt.apply_forces(system_one, system_one_index, system_two, system_two_index) + frjt.apply_torques(system_one, system_one_index, system_two, system_two_index) + + assert_allclose( + system_one.external_forces[..., system_one_index], + external_force_system_one, + atol=Tolerance.atol(), + ) + assert_allclose( + system_two.external_forces[..., system_two_index], + external_force_system_two, + atol=Tolerance.atol(), + ) + + # these checks are only possible because the system is not rotated + # if system is rotated, first distance between CoM and joint connection point would need to be computed + # and torque would need to be rotated into local frame at the end. + + # first check torque on system one + if np.array_equal(point_system_one, np.zeros((3, 1))): + # for the joint connected at the node / CoM there shouldn't be any torque generated + assert_allclose( + system_one.external_torques[..., system_one_index], + np.zeros((3,)), + atol=Tolerance.atol(), + ) + elif np.array_equal(point_system_one, np.array([0.0, 0.0, 1.0])): + assert_allclose( + system_one.external_torques[..., system_one_index], + np.array( + [-external_force_system_one[1], external_force_system_one[0], 0.0] + ), + atol=Tolerance.atol(), + ) + else: + assert_allclose( + system_one.external_torques[..., system_one_index], + np.cross(point_system_one, external_force_system_one), + atol=Tolerance.atol(), + ) + + if np.array_equal(point_system_two, np.zeros((3, 1))): + # for the joint connected at the node / CoM there shouldn't be any torque generated + assert_allclose( + system_two.external_torques[..., system_two_index], + np.zeros((3,)), + atol=Tolerance.atol(), + ) + elif np.array_equal(point_system_two, np.array([0.0, 0.0, 1.0])): + assert_allclose( + system_two.external_torques[..., system_two_index], + np.array( + [-external_force_system_two[1], external_force_system_two[0], 0.0] + ), + atol=Tolerance.atol(), + ) + else: + assert_allclose( + system_two.external_torques[..., system_two_index], + np.cross(point_system_two, external_force_system_two), + atol=Tolerance.atol(), + ) diff --git a/tests/test_joint.py b/tests/test_joint.py index 7c56e7372..b12c179b3 100644 --- a/tests/test_joint.py +++ b/tests/test_joint.py @@ -1,19 +1,33 @@ __doc__ = """ Joint between rods test module """ # System imports -import numpy as np -from elastica.joint import FreeJoint, HingeJoint, FixedJoint +import elastica +from elastica.experimental.connection_contact_joint.generic_system_type_connection import ( + compute_position_of_point, + compute_velocity_of_point, +) +from elastica.joint import ( + FreeJoint, + HingeJoint, + FixedJoint, +) from numpy.testing import assert_allclose -from elastica.utils import Tolerance +from elastica._rotations import _inv_rotate +from elastica.rigidbody import Cylinder from elastica.rod.cosserat_rod import CosseratRod -import elastica +from elastica.utils import Tolerance import importlib +import numpy as np import pytest from scipy.spatial.transform import Rotation # TODO: change tests and made them independent of rod, at least assigin hardcoded values for forces and torques +# seed random number generator +rng = np.random.default_rng(0) + + def test_freejoint(): # Some rod properties. We need them for constructer, they are not used. @@ -207,12 +221,9 @@ def test_hingejoint(): rod2.external_forces[..., rod2_index], -1 * contactforce, atol=Tolerance.atol() ) - linkdirection = ( - rod2.position_collection[..., rod2_index + 1] - - rod2.position_collection[..., rod2_index] - ) - forcedirection = np.dot(linkdirection, normal1) * normal1 - torque = -kt * np.cross(linkdirection, forcedirection) + system_two_tangent = rod2.director_collection[2, :, rod2_index] + force_direction = np.dot(system_two_tangent, normal1) * normal1 + torque = -kt * np.cross(system_two_tangent, force_direction) torque_rod1 = -rod1.director_collection[..., rod1_index] @ torque torque_rod2 = rod2.director_collection[..., rod2_index] @ torque @@ -230,7 +241,7 @@ def test_hingejoint(): np.array([np.pi / 2, 0.0, 0.0]), np.array([0.0, np.pi / 2, 0.0]), np.array([0.0, 0.0, np.pi / 2]), - 2 * np.pi * np.random.random_sample(size=3), + 2 * np.pi * rng.random(size=3), ] @@ -361,7 +372,7 @@ def test_fixedjoint(rest_euler_angle): # compute rotation vectors based on C_22* # scipy implementation - rot_vec = Rotation.from_matrix(dev_rot).as_rotvec() + rot_vec = _inv_rotate(np.dstack([np.eye(3), dev_rot.T])).squeeze() # rotate rotation vector into inertial frame rot_vec_inertial_frame = rod2_director.T @ rot_vec