From 0ccba62afc5bf3f9b90144df692722c62eb1db14 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Thu, 14 Jul 2022 12:09:21 +0200 Subject: [PATCH 01/37] Fix #131 for FixedJoints by applying restoring torques on rotation errors using rotational spring-damper systems --- elastica/joint.py | 60 ++++---- examples/JointCases/fixed_joint_torsion.py | 152 ++++++++++++++++++++ examples/JointCases/joint_cases_callback.py | 22 +++ 3 files changed, 203 insertions(+), 31 deletions(-) create mode 100644 examples/JointCases/fixed_joint_torsion.py create mode 100644 examples/JointCases/joint_cases_callback.py diff --git a/elastica/joint.py b/elastica/joint.py index 0f9eb8415..37d720e3a 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -5,6 +5,7 @@ from elastica.utils import Tolerance, MaxDimension from elastica._linalg import _batch_product_k_ik_to_ik from math import sqrt +from scipy.spatial.transform import Rotation class FreeJoint: @@ -209,9 +210,11 @@ class FixedJoint(FreeJoint): Damping coefficient of the joint. kt: float Rotational stiffness coefficient of the joint. + nut: float + Rotational damping coefficient of the joint. """ - def __init__(self, k, nu, kt): + def __init__(self, k, nu, kt, nut=0.): """ Parameters @@ -222,51 +225,46 @@ def __init__(self, k, nu, kt): Damping coefficient of the joint. kt: float Rotational stiffness coefficient of the joint. + nut: float = 0. + Rotational damping coefficient of the joint. """ super().__init__(k, nu) # additional in-plane constraint through restoring torque # stiffness of the restoring constraint -- tuned empirically self.kt = kt + self.nut = nut # 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_torques(self, rod_one, index_one, rod_two, index_two): - # current direction of the first element of link two - # also NOTE: - rod two is fixed 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): + # collect directors of systems one and two + # note that systems can be either rods or rigid bodies + system_one_director = system_one.director_collection[..., index_one] + system_two_director = system_two.director_collection[..., index_two] - # To constrain the orientation of link two, the second node of link two should align with - # the direction of link one. Thus, we compute the desired position of the second node of link two - # as check1, and the current position of the second node of link two as check2. Check1 and check2 - # should overlap. + # relative rotation matrix from system 1 to system 2: C_12 = C_1W @ C_W2 where W denotes the inertial frame + error_rot = system_one_director @ system_two_director.T - tgt_destination = ( - rod_one.position_collection[..., index_one] - + rod_two.rest_lengths[index_two] * rod_one.tangents[..., index_one] - ) # dl of rod 2 can be different than rod 1 so use rest length of rod 2 + # compute rotation vector from system one to system two based on relative rotation matrix + rot_vec = Rotation.from_matrix(error_rot).as_rotvec(degrees=False) - curr_destination = rod_two.position_collection[ - ..., index_two + 1 - ] # second element of rod2 + # rotate rotation vector into inertial frame + rot_vec = system_one_director.T @ rot_vec - # Compute the restoring torque - forcedirection = -self.kt * ( - curr_destination - tgt_destination - ) # force direction is between rod2 2nd element and rod1 - torque = np.cross(link_direction, forcedirection) + # we compute the constraining torque using a rotational spring - damper system in the inertial frame + torque = self.kt * rot_vec - # The opposite torque will be applied on link one - rod_one.external_torques[..., index_one] -= ( - rod_one.director_collection[..., index_one] @ torque - ) - rod_two.external_torques[..., index_two] += ( - rod_two.director_collection[..., index_two] @ torque - ) + # add damping torque + if self.nut > 0.: + # error in rotation velocity between system 1 and system 2 + error_omega = system_two.omega_collection[..., index_two] - system_one.omega_collection[..., index_one] + torque += self.nut * error_omega + + # The opposite torques will be applied to system one and two after rotating the torques into the local frame + system_one.external_torques[..., index_one] += system_one_director @ torque + system_two.external_torques[..., index_two] -= system_two_director @ torque @numba.njit(cache=True) diff --git a/examples/JointCases/fixed_joint_torsion.py b/examples/JointCases/fixed_joint_torsion.py new file mode 100644 index 000000000..acb100024 --- /dev/null +++ b/examples/JointCases/fixed_joint_torsion.py @@ -0,0 +1,152 @@ +__doc__ = """Fixed joint example, for detailed explanation refer to Zhang et. al. Nature Comm. methods section.""" + +import matplotlib.pyplot as plt +import numpy as np +import sys + +# FIXME without appending sys.path make it more generic +sys.path.append("../../") +from elastica import * +from examples.JointCases.external_force_class_for_joint_test import ( + EndpointForcesSinusoidal, +) +from examples.JointCases.joint_cases_callback import JointCasesCallback +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) + +start_rod_1 = np.zeros((3,)) +start_rod_2 = start_rod_1 + 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) + +# 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(FixedJoint, k=1e5, nu=0, kt=1e3) + +# Add forces to rod2 +fixed_joint_sim.add_forcing_to(rod2).using( + UniformTorques, + torque=5e-3, + direction=np.array([0., 0., 1.]) +) + +# add damping +damping_constant = 0.4 +dt = 1e-5 +fixed_joint_sim.dampen(rod1).using( + ExponentialDamper, + damping_constant=damping_constant, + time_step=dt, +) +fixed_joint_sim.dampen(rod2).using( + ExponentialDamper, + damping_constant=damping_constant, + time_step=dt, +) + + +pp_list_rod1 = defaultdict(list) +pp_list_rod2 = defaultdict(list) + + +fixed_joint_sim.collect_diagnostics(rod1).using( + JointCasesCallback, step_skip=1000, callback_params=pp_list_rod1 +) +fixed_joint_sim.collect_diagnostics(rod2).using( + JointCasesCallback, step_skip=1000, callback_params=pp_list_rod2 +) + +fixed_joint_sim.finalize() +timestepper = PositionVerlet() + +final_time = 10 +dl = base_length / n_elem +dt = 1e-5 +total_steps = int(final_time / dt) +print("Total steps", total_steps) +integrate(timestepper, fixed_joint_sim, final_time, total_steps) + + +def plot_orientation_vs_time(title, time, directors): + from scipy.spatial.transform import Rotation as R + quat = [] + for t in range(len(time)): + quat_t = R.from_matrix(directors[t].T).as_quat() + quat.append(quat_t) + quat = np.array(quat) + + plt.figure(num=title) + plt.plot(time, quat[:, 0], label="x") + plt.plot(time, quat[:, 1], label="y") + plt.plot(time, quat[:, 2], label="z") + plt.plot(time, quat[:, 3], label="w") + plt.title(title) + plt.legend() + plt.xlabel("Time [s]") + plt.ylabel("Quaternion") + plt.show() + + +plot_orientation_vs_time("Orientation of last node of rod 1", pp_list_rod1["time"], + np.array(pp_list_rod1["director"])[..., -1]) +plot_orientation_vs_time("Orientation of last node of rod 2", pp_list_rod2["time"], + np.array(pp_list_rod2["director"])[..., -1]) \ No newline at end of file diff --git a/examples/JointCases/joint_cases_callback.py b/examples/JointCases/joint_cases_callback.py new file mode 100644 index 000000000..969a85496 --- /dev/null +++ b/examples/JointCases/joint_cases_callback.py @@ -0,0 +1,22 @@ +from elastica.callback_functions import CallBackBaseClass + + +class JointCasesCallback(CallBackBaseClass): + """ + Call back function for testing joints + """ + + def __init__(self, step_skip: int, callback_params: dict): + CallBackBaseClass.__init__(self) + self.every = step_skip + self.callback_params = callback_params + + def make_callback(self, system, time, current_step: int): + if current_step % self.every == 0: + self.callback_params["time"].append(time) + self.callback_params["step"].append(current_step) + self.callback_params["position"].append(system.position_collection.copy()) + self.callback_params["director"].append(system.director_collection.copy()) + self.callback_params["velocity"].append(system.velocity_collection.copy()) + + return \ No newline at end of file From e7d1b224699b42f82e4be29fbeb279a5ec09a062 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Thu, 14 Jul 2022 12:30:01 +0200 Subject: [PATCH 02/37] Use JointCasesCallback also for remaining joint cases examples --- examples/JointCases/fixed_joint.py | 27 +++----------------- examples/JointCases/hinge_joint.py | 28 +++------------------ examples/JointCases/joint_cases_callback.py | 2 +- examples/JointCases/spherical_joint.py | 28 +++------------------ 4 files changed, 10 insertions(+), 75 deletions(-) diff --git a/examples/JointCases/fixed_joint.py b/examples/JointCases/fixed_joint.py index 449190a4c..b00e50d7a 100644 --- a/examples/JointCases/fixed_joint.py +++ b/examples/JointCases/fixed_joint.py @@ -9,6 +9,7 @@ from examples.JointCases.external_force_class_for_joint_test import ( EndpointForcesSinusoidal, ) +from examples.JointCases.joint_cases_callback import JointCasesCallback from examples.JointCases.joint_cases_postprocessing import ( plot_position, plot_video, @@ -104,36 +105,14 @@ class FixedJointSimulator( time_step=dt, ) -# Callback functions -# Add call backs -class TestJoints(CallBackBaseClass): - """ - Call back function for testing joints - """ - - def __init__(self, step_skip: int, callback_params: dict): - CallBackBaseClass.__init__(self) - self.every = step_skip - self.callback_params = callback_params - - def make_callback(self, system, time, current_step: int): - if current_step % self.every == 0: - self.callback_params["time"].append(time) - self.callback_params["step"].append(current_step) - self.callback_params["position"].append(system.position_collection.copy()) - self.callback_params["velocity"].append(system.velocity_collection.copy()) - return - - pp_list_rod1 = defaultdict(list) pp_list_rod2 = defaultdict(list) - fixed_joint_sim.collect_diagnostics(rod1).using( - TestJoints, step_skip=1000, callback_params=pp_list_rod1 + JointCasesCallback, step_skip=1000, callback_params=pp_list_rod1 ) fixed_joint_sim.collect_diagnostics(rod2).using( - TestJoints, step_skip=1000, callback_params=pp_list_rod2 + JointCasesCallback, step_skip=1000, callback_params=pp_list_rod2 ) fixed_joint_sim.finalize() diff --git a/examples/JointCases/hinge_joint.py b/examples/JointCases/hinge_joint.py index bb37dc7e6..2febcdae2 100644 --- a/examples/JointCases/hinge_joint.py +++ b/examples/JointCases/hinge_joint.py @@ -9,6 +9,7 @@ from examples.JointCases.external_force_class_for_joint_test import ( EndpointForcesSinusoidal, ) +from examples.JointCases.joint_cases_callback import JointCasesCallback from examples.JointCases.joint_cases_postprocessing import ( plot_position, plot_video, @@ -106,39 +107,16 @@ class HingeJointSimulator( time_step=dt, ) -# Callback functions -# Add call backs -class TestJoints(CallBackBaseClass): - """ - Call back function for testing joints - """ - - def __init__(self, step_skip: int, callback_params: dict): - CallBackBaseClass.__init__(self) - self.every = step_skip - self.callback_params = callback_params - - def make_callback(self, system, time, current_step: int): - if current_step % self.every == 0: - self.callback_params["time"].append(time) - self.callback_params["step"].append(current_step) - self.callback_params["position"].append(system.position_collection.copy()) - self.callback_params["velocity"].append(system.velocity_collection.copy()) - return - - pp_list_rod1 = defaultdict(list) pp_list_rod2 = defaultdict(list) - hinge_joint_sim.collect_diagnostics(rod1).using( - TestJoints, step_skip=1000, callback_params=pp_list_rod1 + JointCasesCallback, step_skip=1000, callback_params=pp_list_rod1 ) hinge_joint_sim.collect_diagnostics(rod2).using( - TestJoints, step_skip=1000, callback_params=pp_list_rod2 + JointCasesCallback, step_skip=1000, callback_params=pp_list_rod2 ) - hinge_joint_sim.finalize() timestepper = PositionVerlet() # timestepper = PEFRL() diff --git a/examples/JointCases/joint_cases_callback.py b/examples/JointCases/joint_cases_callback.py index 969a85496..8cf4e90c4 100644 --- a/examples/JointCases/joint_cases_callback.py +++ b/examples/JointCases/joint_cases_callback.py @@ -19,4 +19,4 @@ def make_callback(self, system, time, current_step: int): self.callback_params["director"].append(system.director_collection.copy()) self.callback_params["velocity"].append(system.velocity_collection.copy()) - return \ No newline at end of file + return diff --git a/examples/JointCases/spherical_joint.py b/examples/JointCases/spherical_joint.py index d9771f37e..1315d048c 100644 --- a/examples/JointCases/spherical_joint.py +++ b/examples/JointCases/spherical_joint.py @@ -10,6 +10,7 @@ from examples.JointCases.external_force_class_for_joint_test import ( EndpointForcesSinusoidal, ) +from examples.JointCases.joint_cases_callback import JointCasesCallback from examples.JointCases.joint_cases_postprocessing import ( plot_position, plot_video, @@ -107,37 +108,14 @@ class SphericalJointSimulator( time_step=dt, ) - -# Callback functions -# Add call backs -class TestJoints(CallBackBaseClass): - """ - Call back function for testing joints - """ - - def __init__(self, step_skip: int, callback_params: dict): - CallBackBaseClass.__init__(self) - self.every = step_skip - self.callback_params = callback_params - - def make_callback(self, system, time, current_step: int): - if current_step % self.every == 0: - self.callback_params["time"].append(time) - self.callback_params["step"].append(current_step) - self.callback_params["position"].append(system.position_collection.copy()) - self.callback_params["velocity"].append(system.velocity_collection.copy()) - return - - pp_list_rod1 = defaultdict(list) pp_list_rod2 = defaultdict(list) - spherical_joint_sim.collect_diagnostics(rod1).using( - TestJoints, step_skip=1000, callback_params=pp_list_rod1 + JointCasesCallback, step_skip=1000, callback_params=pp_list_rod1 ) spherical_joint_sim.collect_diagnostics(rod2).using( - TestJoints, step_skip=1000, callback_params=pp_list_rod2 + JointCasesCallback, step_skip=1000, callback_params=pp_list_rod2 ) spherical_joint_sim.finalize() From b72bdc9992689682bb6dfd7fbf5f103e1172199f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Thu, 14 Jul 2022 14:00:52 +0200 Subject: [PATCH 03/37] Formatting using black --- elastica/joint.py | 9 ++++++--- examples/JointCases/fixed_joint_torsion.py | 20 ++++++++++++-------- 2 files changed, 18 insertions(+), 11 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 37d720e3a..dcb818330 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -214,7 +214,7 @@ class FixedJoint(FreeJoint): Rotational damping coefficient of the joint. """ - def __init__(self, k, nu, kt, nut=0.): + def __init__(self, k, nu, kt, nut=0.0): """ Parameters @@ -257,9 +257,12 @@ def apply_torques(self, system_one, index_one, system_two, index_two): torque = self.kt * rot_vec # add damping torque - if self.nut > 0.: + if self.nut > 0.0: # error in rotation velocity between system 1 and system 2 - error_omega = system_two.omega_collection[..., index_two] - system_one.omega_collection[..., index_one] + error_omega = ( + system_two.omega_collection[..., index_two] + - system_one.omega_collection[..., index_one] + ) torque += self.nut * error_omega # The opposite torques will be applied to system one and two after rotating the torques into the local frame diff --git a/examples/JointCases/fixed_joint_torsion.py b/examples/JointCases/fixed_joint_torsion.py index acb100024..273467624 100644 --- a/examples/JointCases/fixed_joint_torsion.py +++ b/examples/JointCases/fixed_joint_torsion.py @@ -2,6 +2,7 @@ import matplotlib.pyplot as plt import numpy as np +from scipy.spatial.transform import Rotation as R import sys # FIXME without appending sys.path make it more generic @@ -84,9 +85,7 @@ class FixedJointSimulator( # Add forces to rod2 fixed_joint_sim.add_forcing_to(rod2).using( - UniformTorques, - torque=5e-3, - direction=np.array([0., 0., 1.]) + UniformTorques, torque=5e-3, direction=np.array([0.0, 0.0, 1.0]) ) # add damping @@ -127,7 +126,6 @@ class FixedJointSimulator( def plot_orientation_vs_time(title, time, directors): - from scipy.spatial.transform import Rotation as R quat = [] for t in range(len(time)): quat_t = R.from_matrix(directors[t].T).as_quat() @@ -146,7 +144,13 @@ def plot_orientation_vs_time(title, time, directors): plt.show() -plot_orientation_vs_time("Orientation of last node of rod 1", pp_list_rod1["time"], - np.array(pp_list_rod1["director"])[..., -1]) -plot_orientation_vs_time("Orientation of last node of rod 2", pp_list_rod2["time"], - np.array(pp_list_rod2["director"])[..., -1]) \ No newline at end of file +plot_orientation_vs_time( + "Orientation of last node of rod 1", + pp_list_rod1["time"], + np.array(pp_list_rod1["director"])[..., -1], +) +plot_orientation_vs_time( + "Orientation of last node of rod 2", + pp_list_rod2["time"], + np.array(pp_list_rod2["director"])[..., -1], +) From a27ee9950c726dd89ecd0b948c49109dd55ad97b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Thu, 14 Jul 2022 14:05:22 +0200 Subject: [PATCH 04/37] Scipy as_rotvec has issue in documentation about degrees parameter --- elastica/joint.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/elastica/joint.py b/elastica/joint.py index dcb818330..9550aa6cc 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -248,7 +248,7 @@ def apply_torques(self, system_one, index_one, system_two, index_two): error_rot = system_one_director @ system_two_director.T # compute rotation vector from system one to system two based on relative rotation matrix - rot_vec = Rotation.from_matrix(error_rot).as_rotvec(degrees=False) + rot_vec = Rotation.from_matrix(error_rot).as_rotvec() # rotate rotation vector into inertial frame rot_vec = system_one_director.T @ rot_vec From 6c8ac2560e267f96fdf8cc6b412d4581514e521d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Thu, 14 Jul 2022 15:28:47 +0200 Subject: [PATCH 05/37] Adjust test for FixedJoint --- tests/test_joint.py | 47 +++++++++++++++++++++++++-------------------- 1 file changed, 26 insertions(+), 21 deletions(-) diff --git a/tests/test_joint.py b/tests/test_joint.py index 042b32424..67613058d 100644 --- a/tests/test_joint.py +++ b/tests/test_joint.py @@ -8,6 +8,7 @@ from elastica.rod.cosserat_rod import CosseratRod import importlib import elastica +from scipy.spatial.transform import Rotation # TODO: change tests and made them independent of rod, at least assigin hardcoded values for forces and torques @@ -282,11 +283,12 @@ def test_fixedjoint(): rod1.velocity_collection = np.tile(v1, (1, n + 1)) rod2.velocity_collection = np.tile(v2, (1, n + 1)) - # Stiffness between points + # Positional and rotational stiffness between systems k = 1e8 kt = 1e6 - # Damping between two points + # Positional and rotational damping between systems nu = 1 + nut = 1 # Rod indexes rod1_index = -1 @@ -321,34 +323,37 @@ def test_fixedjoint(): rod2.external_forces[..., rod2_index], -1 * contactforce, atol=Tolerance.atol() ) - linkdirection = ( - rod2.position_collection[..., rod2_index + 1] - - rod2.position_collection[..., rod2_index] - ) + # collect directors of systems one and two + # note that systems can be either rods or rigid bodies + rod1_director = rod1.director_collection[..., rod1_index] + rod2_director = rod2.director_collection[..., rod2_index] - positiondiff = ( - rod1.position_collection[..., rod1_index] - - rod1.position_collection[..., rod1_index - 1] - ) - tangent = positiondiff / np.sqrt(np.dot(positiondiff, positiondiff)) + # relative rotation matrix from system 1 to system 2: C_12 = C_1W @ C_W2 where W denotes the inertial frame + error_rot = rod1_director @ rod2_director.T - # rod 2 has to be alligned with rod 1 - check1 = ( - rod1.position_collection[..., rod1_index] - + rod2.rest_lengths[rod2_index] * tangent + # compute rotation vector from system one to system two based on relative rotation matrix + rot_vec = Rotation.from_matrix(error_rot).as_rotvec() + + # rotate rotation vector into inertial frame + rot_vec = rod1_director.T @ rot_vec + + # error in rotation velocity between system 1 and system 2 + error_omega = ( + rod2.omega_collection[..., rod2_index] - rod1.omega_collection[..., rod2_index] ) - check2 = rod2.position_collection[..., rod2_index + 1] - forcedirection = -kt * (check2 - check1) - torque = np.cross(linkdirection, forcedirection) - torque_rod1 = -rod1.director_collection[..., rod1_index] @ torque - torque_rod2 = rod2.director_collection[..., rod2_index] @ torque + # we compute the constraining torque using a rotational spring - damper system in the inertial frame + torque = kt * rot_vec + nut * error_omega + + # The opposite torques will be applied to system one and two after rotating the torques into the local frame + torque_rod1 = rod1_director @ torque + torque_rod2 = rod2_director @ torque assert_allclose( rod1.external_torques[..., rod1_index], torque_rod1, atol=Tolerance.atol() ) assert_allclose( - rod2.external_torques[..., rod2_index], torque_rod2, atol=Tolerance.atol() + rod2.external_torques[..., rod2_index], -torque_rod2, atol=Tolerance.atol() ) From 187bb098e08731d64dc8dc72ca5a738b0d43a714 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Thu, 14 Jul 2022 16:05:06 +0200 Subject: [PATCH 06/37] Fix direction of compensation damping torque for FixedJoint --- elastica/joint.py | 2 +- examples/JointCases/fixed_joint_torsion.py | 4 ++-- tests/test_joint.py | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 9550aa6cc..578b94622 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -263,7 +263,7 @@ def apply_torques(self, system_one, index_one, system_two, index_two): system_two.omega_collection[..., index_two] - system_one.omega_collection[..., index_one] ) - torque += self.nut * error_omega + torque -= self.nut * error_omega # The opposite torques will be applied to system one and two after rotating the torques into the local frame system_one.external_torques[..., index_one] += system_one_director @ torque diff --git a/examples/JointCases/fixed_joint_torsion.py b/examples/JointCases/fixed_joint_torsion.py index 273467624..4b83863d4 100644 --- a/examples/JointCases/fixed_joint_torsion.py +++ b/examples/JointCases/fixed_joint_torsion.py @@ -81,7 +81,7 @@ 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=1e3) +).using(FixedJoint, k=1e5, nu=1., kt=1e3, nut=1e-3) # Add forces to rod2 fixed_joint_sim.add_forcing_to(rod2).using( @@ -117,7 +117,7 @@ class FixedJointSimulator( fixed_joint_sim.finalize() timestepper = PositionVerlet() -final_time = 10 +final_time = 1 dl = base_length / n_elem dt = 1e-5 total_steps = int(final_time / dt) diff --git a/tests/test_joint.py b/tests/test_joint.py index 67613058d..497aa60df 100644 --- a/tests/test_joint.py +++ b/tests/test_joint.py @@ -288,7 +288,7 @@ def test_fixedjoint(): kt = 1e6 # Positional and rotational damping between systems nu = 1 - nut = 1 + nut = 1e-2 # Rod indexes rod1_index = -1 @@ -343,7 +343,7 @@ def test_fixedjoint(): ) # we compute the constraining torque using a rotational spring - damper system in the inertial frame - torque = kt * rot_vec + nut * error_omega + torque = kt * rot_vec - nut * error_omega # The opposite torques will be applied to system one and two after rotating the torques into the local frame torque_rod1 = rod1_director @ torque From 368be52d54744862bd2b4b9ec6547ab3725fbe7d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Fri, 15 Jul 2022 14:15:26 +0200 Subject: [PATCH 07/37] Improve plotting for `fixed_joint_torsion` example --- examples/JointCases/fixed_joint_torsion.py | 63 ++++++++++++------- .../JointCases/joint_cases_postprocessing.py | 24 ++++++- 2 files changed, 61 insertions(+), 26 deletions(-) diff --git a/examples/JointCases/fixed_joint_torsion.py b/examples/JointCases/fixed_joint_torsion.py index 4b83863d4..8b4fbcf33 100644 --- a/examples/JointCases/fixed_joint_torsion.py +++ b/examples/JointCases/fixed_joint_torsion.py @@ -2,7 +2,6 @@ import matplotlib.pyplot as plt import numpy as np -from scipy.spatial.transform import Rotation as R import sys # FIXME without appending sys.path make it more generic @@ -14,6 +13,7 @@ from examples.JointCases.joint_cases_callback import JointCasesCallback from examples.JointCases.joint_cases_postprocessing import ( plot_position, + plot_orientation, plot_video, plot_video_xy, plot_video_xz, @@ -81,7 +81,7 @@ 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=1., kt=1e3, nut=1e-3) +).using(FixedJoint, k=1e5, nu=1.0, kt=1e3, nut=1e-3) # Add forces to rod2 fixed_joint_sim.add_forcing_to(rod2).using( @@ -117,7 +117,7 @@ class FixedJointSimulator( fixed_joint_sim.finalize() timestepper = PositionVerlet() -final_time = 1 +final_time = 10 dl = base_length / n_elem dt = 1e-5 total_steps = int(final_time / dt) @@ -125,32 +125,47 @@ class FixedJointSimulator( integrate(timestepper, fixed_joint_sim, final_time, total_steps) -def plot_orientation_vs_time(title, time, directors): - quat = [] - for t in range(len(time)): - quat_t = R.from_matrix(directors[t].T).as_quat() - quat.append(quat_t) - quat = np.array(quat) - - plt.figure(num=title) - plt.plot(time, quat[:, 0], label="x") - plt.plot(time, quat[:, 1], label="y") - plt.plot(time, quat[:, 2], label="z") - plt.plot(time, quat[:, 3], label="w") - plt.title(title) - plt.legend() - plt.xlabel("Time [s]") - plt.ylabel("Quaternion") - plt.show() - - -plot_orientation_vs_time( +plot_orientation( "Orientation of last node of rod 1", pp_list_rod1["time"], np.array(pp_list_rod1["director"])[..., -1], ) -plot_orientation_vs_time( +plot_orientation( "Orientation of last node of rod 2", pp_list_rod2["time"], np.array(pp_list_rod2["director"])[..., -1], ) + +PLOT_FIGURE = True +SAVE_FIGURE = True +PLOT_VIDEO = True + +# plotting results +if PLOT_FIGURE: + filename = "fixed_joint_torsion_test.png" + plot_position(pp_list_rod1, pp_list_rod2, filename, SAVE_FIGURE) + +if PLOT_VIDEO: + filename = "fixed_joint_torsion_test" + fps = 100 # Hz + plot_video( + pp_list_rod1, + pp_list_rod2, + video_name=filename + ".mp4", + margin=0.2, + fps=fps, + ) + plot_video_xy( + pp_list_rod1, + pp_list_rod2, + video_name=filename + "_xy.mp4", + margin=0.2, + fps=fps, + ) + plot_video_xz( + pp_list_rod1, + pp_list_rod2, + video_name=filename + "_xz.mp4", + margin=0.2, + fps=fps, + ) diff --git a/examples/JointCases/joint_cases_postprocessing.py b/examples/JointCases/joint_cases_postprocessing.py index 87eb357b7..d7ea14376 100644 --- a/examples/JointCases/joint_cases_postprocessing.py +++ b/examples/JointCases/joint_cases_postprocessing.py @@ -2,6 +2,7 @@ from matplotlib import pyplot as plt from matplotlib.colors import to_rgb from mpl_toolkits import mplot3d +from scipy.spatial.transform import Rotation def plot_position( @@ -35,6 +36,25 @@ def plot_position( fig.savefig(filename) +def plot_orientation(title, time, directors): + quat = [] + for t in range(len(time)): + quat_t = Rotation.from_matrix(directors[t].T).as_quat() + quat.append(quat_t) + quat = np.array(quat) + + plt.figure(num=title) + plt.plot(time, quat[:, 0], label="x") + plt.plot(time, quat[:, 1], label="y") + plt.plot(time, quat[:, 2], label="z") + plt.plot(time, quat[:, 3], label="w") + plt.title(title) + plt.legend() + plt.xlabel("Time [s]") + plt.ylabel("Quaternion") + plt.show() + + def plot_video( plot_params_rod1: dict, plot_params_rod2: dict, @@ -94,7 +114,7 @@ def plot_video_xy( position_of_rod1 = np.array(plot_params_rod1["position"]) position_of_rod2 = np.array(plot_params_rod2["position"]) - print("plot video") + print("plot video xy") FFMpegWriter = manimation.writers["ffmpeg"] metadata = dict(title="Movie Test", artist="Matplotlib", comment="Movie support!") writer = FFMpegWriter(fps=fps, metadata=metadata) @@ -132,7 +152,7 @@ def plot_video_xz( position_of_rod1 = np.array(plot_params_rod1["position"]) position_of_rod2 = np.array(plot_params_rod2["position"]) - print("plot video") + print("plot video xz") FFMpegWriter = manimation.writers["ffmpeg"] metadata = dict(title="Movie Test", artist="Matplotlib", comment="Movie support!") writer = FFMpegWriter(fps=fps, metadata=metadata) From 21078056d8b2133d42c09591182ac3374aed3378 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Fri, 15 Jul 2022 14:23:52 +0200 Subject: [PATCH 08/37] Enable the FixedJoint to automatically configure a static rotation offset --- elastica/joint.py | 33 +++++++++++++++++----- examples/JointCases/fixed_joint_torsion.py | 2 +- 2 files changed, 27 insertions(+), 8 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 578b94622..96d980f38 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -212,9 +212,12 @@ class FixedJoint(FreeJoint): Rotational stiffness coefficient of the joint. nut: float Rotational damping coefficient of the joint. + static_rotation: np.array + Static 3x3 rotation matrix from system one to system two. + Instead of aligning the directors of both systems directly, a desired rotational offset C_12* is enforced. """ - def __init__(self, k, nu, kt, nut=0.0): + def __init__(self, k, nu, kt, nut=0.0, use_static_rotation=True): """ Parameters @@ -227,6 +230,9 @@ def __init__(self, k, nu, kt, nut=0.0): Rotational stiffness coefficient of the joint. nut: float = 0. Rotational damping coefficient of the joint. + use_static_rotation: bool = True + If True, the initial rotation offset between the two systems is recorded + and enforced throughout the entire simulation. """ super().__init__(k, nu) # additional in-plane constraint through restoring torque @@ -234,6 +240,13 @@ def __init__(self, k, nu, kt, nut=0.0): self.kt = kt self.nut = nut + # if a static rotation offset between the two systems should not be enforced, + # we set the relative rotation to the identity matrix. Otherwise + if use_static_rotation: + self.static_rotation = None + else: + self.static_rotation = np.eye(3) + # 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) @@ -244,14 +257,20 @@ def apply_torques(self, system_one, index_one, system_two, index_two): system_one_director = system_one.director_collection[..., index_one] system_two_director = system_two.director_collection[..., index_two] - # relative rotation matrix from system 1 to system 2: C_12 = C_1W @ C_W2 where W denotes the inertial frame - error_rot = system_one_director @ system_two_director.T + if self.static_rotation is None: + # this if clause should be active during the first timestep for the case use_static_rotation==True + self.static_rotation = system_one_director @ system_two_director.T + + # relative rotation matrix from system 1 to system 2: C_12 = C_1I @ C_I2 where I denotes the inertial frame + rel_rot = system_one_director @ system_two_director.T + # C_22* = C_21 @ C_12* where C_12* is the desired rotation between systems one and two + error_rot = rel_rot.T @ self.static_rotation - # compute rotation vector from system one to system two based on relative rotation matrix + # compute rotation vectors based on C_22* rot_vec = Rotation.from_matrix(error_rot).as_rotvec() # rotate rotation vector into inertial frame - rot_vec = system_one_director.T @ rot_vec + rot_vec = system_two_director.T @ rot_vec # we compute the constraining torque using a rotational spring - damper system in the inertial frame torque = self.kt * rot_vec @@ -266,8 +285,8 @@ def apply_torques(self, system_one, index_one, system_two, index_two): torque -= self.nut * error_omega # The opposite torques will be applied to system one and two after rotating the torques into the local frame - system_one.external_torques[..., index_one] += system_one_director @ torque - system_two.external_torques[..., index_two] -= system_two_director @ torque + system_one.external_torques[..., index_one] -= system_one_director @ torque + system_two.external_torques[..., index_two] += system_two_director @ torque @numba.njit(cache=True) diff --git a/examples/JointCases/fixed_joint_torsion.py b/examples/JointCases/fixed_joint_torsion.py index 8b4fbcf33..54c51dc01 100644 --- a/examples/JointCases/fixed_joint_torsion.py +++ b/examples/JointCases/fixed_joint_torsion.py @@ -117,7 +117,7 @@ class FixedJointSimulator( fixed_joint_sim.finalize() timestepper = PositionVerlet() -final_time = 10 +final_time = 1 dl = base_length / n_elem dt = 1e-5 total_steps = int(final_time / dt) From 5ddeaa1637ae350c6f09b91db2bd739b21118c29 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Fri, 15 Jul 2022 14:24:35 +0200 Subject: [PATCH 09/37] Fix final time of `fixed_joint_torsion` example --- examples/JointCases/fixed_joint_torsion.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/JointCases/fixed_joint_torsion.py b/examples/JointCases/fixed_joint_torsion.py index 54c51dc01..8b4fbcf33 100644 --- a/examples/JointCases/fixed_joint_torsion.py +++ b/examples/JointCases/fixed_joint_torsion.py @@ -117,7 +117,7 @@ class FixedJointSimulator( fixed_joint_sim.finalize() timestepper = PositionVerlet() -final_time = 1 +final_time = 10 dl = base_length / n_elem dt = 1e-5 total_steps = int(final_time / dt) From 1b2112066db3e4140ddbb4043c837fee9d45a2ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Fri, 15 Jul 2022 14:45:58 +0200 Subject: [PATCH 10/37] Fix test of FixedJoint - remove static rotation from test - Improve checking of nut damping - Fix sign error for computing restoring rotational damping torque --- elastica/joint.py | 4 ++-- tests/test_joint.py | 12 +++++++++--- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 96d980f38..14d50b4ea 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -279,8 +279,8 @@ def apply_torques(self, system_one, index_one, system_two, index_two): if self.nut > 0.0: # error in rotation velocity between system 1 and system 2 error_omega = ( - system_two.omega_collection[..., index_two] - - system_one.omega_collection[..., index_one] + system_one.omega_collection[..., index_one] + - system_two.omega_collection[..., index_two] ) torque -= self.nut * error_omega diff --git a/tests/test_joint.py b/tests/test_joint.py index 497aa60df..c215c2669 100644 --- a/tests/test_joint.py +++ b/tests/test_joint.py @@ -283,12 +283,18 @@ def test_fixedjoint(): rod1.velocity_collection = np.tile(v1, (1, n + 1)) rod2.velocity_collection = np.tile(v2, (1, n + 1)) + # Rod angular velocity + omega1 = 1 / 180 * np.pi * np.array([0, 0, 1]).reshape(3, 1) + omega2 = -omega1 + rod1.omega_collection = np.tile(omega1, (1, n + 1)) + rod2.omega_collection = np.tile(omega2, (1, n + 1)) + # Positional and rotational stiffness between systems k = 1e8 kt = 1e6 # Positional and rotational damping between systems nu = 1 - nut = 1e-2 + nut = 1e1 # Rod indexes rod1_index = -1 @@ -311,7 +317,7 @@ def test_fixedjoint(): dampingforce = nu * normal_relative_vel * distance / end_distance contactforce = elasticforce - dampingforce - fxjt = FixedJoint(k, nu, kt) + fxjt = FixedJoint(k, nu, kt, nut, use_static_rotation=False) fxjt.apply_forces(rod1, rod1_index, rod2, rod2_index) fxjt.apply_torques(rod1, rod1_index, rod2, rod2_index) @@ -339,7 +345,7 @@ def test_fixedjoint(): # error in rotation velocity between system 1 and system 2 error_omega = ( - rod2.omega_collection[..., rod2_index] - rod1.omega_collection[..., rod2_index] + rod2.omega_collection[..., rod2_index] - rod1.omega_collection[..., rod1_index] ) # we compute the constraining torque using a rotational spring - damper system in the inertial frame From 5b379506e3dda6c2344c945d85759955737108c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Fri, 15 Jul 2022 22:56:14 +0200 Subject: [PATCH 11/37] Replace JointCasesCallback with MyCallback --- examples/JointCases/fixed_joint.py | 5 ++--- examples/JointCases/fixed_joint_torsion.py | 8 ++------ examples/JointCases/hinge_joint.py | 5 ++--- examples/JointCases/spherical_joint.py | 5 ++--- 4 files changed, 8 insertions(+), 15 deletions(-) diff --git a/examples/JointCases/fixed_joint.py b/examples/JointCases/fixed_joint.py index b00e50d7a..a18adbee8 100644 --- a/examples/JointCases/fixed_joint.py +++ b/examples/JointCases/fixed_joint.py @@ -9,7 +9,6 @@ from examples.JointCases.external_force_class_for_joint_test import ( EndpointForcesSinusoidal, ) -from examples.JointCases.joint_cases_callback import JointCasesCallback from examples.JointCases.joint_cases_postprocessing import ( plot_position, plot_video, @@ -109,10 +108,10 @@ class FixedJointSimulator( pp_list_rod2 = defaultdict(list) fixed_joint_sim.collect_diagnostics(rod1).using( - JointCasesCallback, step_skip=1000, callback_params=pp_list_rod1 + MyCallBack, step_skip=1000, callback_params=pp_list_rod1 ) fixed_joint_sim.collect_diagnostics(rod2).using( - JointCasesCallback, step_skip=1000, callback_params=pp_list_rod2 + MyCallBack, step_skip=1000, callback_params=pp_list_rod2 ) fixed_joint_sim.finalize() diff --git a/examples/JointCases/fixed_joint_torsion.py b/examples/JointCases/fixed_joint_torsion.py index 8b4fbcf33..00d271ea3 100644 --- a/examples/JointCases/fixed_joint_torsion.py +++ b/examples/JointCases/fixed_joint_torsion.py @@ -7,10 +7,6 @@ # FIXME without appending sys.path make it more generic sys.path.append("../../") from elastica import * -from examples.JointCases.external_force_class_for_joint_test import ( - EndpointForcesSinusoidal, -) -from examples.JointCases.joint_cases_callback import JointCasesCallback from examples.JointCases.joint_cases_postprocessing import ( plot_position, plot_orientation, @@ -108,10 +104,10 @@ class FixedJointSimulator( fixed_joint_sim.collect_diagnostics(rod1).using( - JointCasesCallback, step_skip=1000, callback_params=pp_list_rod1 + MyCallBack, step_skip=1000, callback_params=pp_list_rod1 ) fixed_joint_sim.collect_diagnostics(rod2).using( - JointCasesCallback, step_skip=1000, callback_params=pp_list_rod2 + MyCallBack, step_skip=1000, callback_params=pp_list_rod2 ) fixed_joint_sim.finalize() diff --git a/examples/JointCases/hinge_joint.py b/examples/JointCases/hinge_joint.py index 2febcdae2..320464f31 100644 --- a/examples/JointCases/hinge_joint.py +++ b/examples/JointCases/hinge_joint.py @@ -9,7 +9,6 @@ from examples.JointCases.external_force_class_for_joint_test import ( EndpointForcesSinusoidal, ) -from examples.JointCases.joint_cases_callback import JointCasesCallback from examples.JointCases.joint_cases_postprocessing import ( plot_position, plot_video, @@ -111,10 +110,10 @@ class HingeJointSimulator( pp_list_rod2 = defaultdict(list) hinge_joint_sim.collect_diagnostics(rod1).using( - JointCasesCallback, step_skip=1000, callback_params=pp_list_rod1 + MyCallBack, step_skip=1000, callback_params=pp_list_rod1 ) hinge_joint_sim.collect_diagnostics(rod2).using( - JointCasesCallback, step_skip=1000, callback_params=pp_list_rod2 + MyCallBack, step_skip=1000, callback_params=pp_list_rod2 ) hinge_joint_sim.finalize() diff --git a/examples/JointCases/spherical_joint.py b/examples/JointCases/spherical_joint.py index 1315d048c..4674b9ea4 100644 --- a/examples/JointCases/spherical_joint.py +++ b/examples/JointCases/spherical_joint.py @@ -10,7 +10,6 @@ from examples.JointCases.external_force_class_for_joint_test import ( EndpointForcesSinusoidal, ) -from examples.JointCases.joint_cases_callback import JointCasesCallback from examples.JointCases.joint_cases_postprocessing import ( plot_position, plot_video, @@ -112,10 +111,10 @@ class SphericalJointSimulator( pp_list_rod2 = defaultdict(list) spherical_joint_sim.collect_diagnostics(rod1).using( - JointCasesCallback, step_skip=1000, callback_params=pp_list_rod1 + MyCallBack, step_skip=1000, callback_params=pp_list_rod1 ) spherical_joint_sim.collect_diagnostics(rod2).using( - JointCasesCallback, step_skip=1000, callback_params=pp_list_rod2 + MyCallBack, step_skip=1000, callback_params=pp_list_rod2 ) spherical_joint_sim.finalize() From d8d7be80bc56ff59f998dfa7e3cbe05dba870591 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Fri, 15 Jul 2022 22:58:57 +0200 Subject: [PATCH 12/37] Remove branching for self.nut > 0.0 --- elastica/joint.py | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 14d50b4ea..4d5feefd5 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -272,17 +272,14 @@ def apply_torques(self, system_one, index_one, system_two, index_two): # rotate rotation vector into inertial frame rot_vec = system_two_director.T @ rot_vec - # we compute the constraining torque using a rotational spring - damper system in the inertial frame - torque = self.kt * rot_vec - - # add damping torque - if self.nut > 0.0: - # error in rotation velocity between system 1 and system 2 - error_omega = ( + # error in rotation velocity between system 1 and system 2 + error_omega = ( system_one.omega_collection[..., index_one] - system_two.omega_collection[..., index_two] - ) - torque -= self.nut * error_omega + ) + + # we compute the constraining torque using a rotational spring - damper system in the inertial frame + torque = self.kt * rot_vec - self.nut * error_omega # The opposite torques will be applied to system one and two after rotating the torques into the local frame system_one.external_torques[..., index_one] -= system_one_director @ torque From 00ecf2f9f0b6ec6cbbe68af802052ed6e7edd932 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Fri, 15 Jul 2022 23:09:07 +0200 Subject: [PATCH 13/37] Fix unclear docstrings --- elastica/joint.py | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 4d5feefd5..2c40958b6 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -230,9 +230,9 @@ def __init__(self, k, nu, kt, nut=0.0, use_static_rotation=True): Rotational stiffness coefficient of the joint. nut: float = 0. Rotational damping coefficient of the joint. - use_static_rotation: bool = True + use_static_rotation: bool If True, the initial rotation offset between the two systems is recorded - and enforced throughout the entire simulation. + and enforced throughout the entire simulation. (default=True) """ super().__init__(k, nu) # additional in-plane constraint through restoring torque @@ -261,9 +261,15 @@ def apply_torques(self, system_one, index_one, system_two, index_two): # this if clause should be active during the first timestep for the case use_static_rotation==True self.static_rotation = system_one_director @ system_two_director.T - # relative rotation matrix from system 1 to system 2: C_12 = C_1I @ C_I2 where I denotes the inertial frame + # rel_rot: C_12 = C_1I @ C_I2 + # C_12 is relative rotation matrix from system 1 to system 2 + # C_1I is the rotation from system 1 to the inertial frame (i.e. the world frame) + # C_I2 is the rotation from the inertial frame to system 2 frame (inverse of system_two_director) rel_rot = system_one_director @ system_two_director.T - # C_22* = C_21 @ C_12* where C_12* is the desired rotation between systems one and two + # error_rot: C_22* = C_21 @ C_12* + # C_22* is rotation matrix from current orientation of system 2 to desired orientation of system 2 + # C_21 is the inverse of C_12, which describes the relative (current) rotation from system 1 to system 2 + # C_12* is the desired rotation between systems one and two, which is saved in the static_rotation attribute error_rot = rel_rot.T @ self.static_rotation # compute rotation vectors based on C_22* @@ -274,8 +280,8 @@ def apply_torques(self, system_one, index_one, system_two, index_two): # error in rotation velocity between system 1 and system 2 error_omega = ( - system_one.omega_collection[..., index_one] - - system_two.omega_collection[..., index_two] + system_one.omega_collection[..., index_one] + - system_two.omega_collection[..., index_two] ) # we compute the constraining torque using a rotational spring - damper system in the inertial frame From cfae6d3a208169d7a67d0bc5ae93a21cde55990f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Fri, 15 Jul 2022 23:10:34 +0200 Subject: [PATCH 14/37] Rename rot_vec in inertial frame to rot_vec_inertial_frame --- elastica/joint.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 2c40958b6..3bcc690d3 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -276,7 +276,7 @@ def apply_torques(self, system_one, index_one, system_two, index_two): rot_vec = Rotation.from_matrix(error_rot).as_rotvec() # rotate rotation vector into inertial frame - rot_vec = system_two_director.T @ rot_vec + rot_vec_inertial_frame = system_two_director.T @ rot_vec # error in rotation velocity between system 1 and system 2 error_omega = ( @@ -285,7 +285,7 @@ def apply_torques(self, system_one, index_one, system_two, index_two): ) # we compute the constraining torque using a rotational spring - damper system in the inertial frame - torque = self.kt * rot_vec - self.nut * error_omega + torque = self.kt * rot_vec_inertial_frame - self.nut * error_omega # The opposite torques will be applied to system one and two after rotating the torques into the local frame system_one.external_torques[..., index_one] -= system_one_director @ torque From b4ae2660a7ddc5e746283100416c0467287d6a7b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 10:02:25 +0200 Subject: [PATCH 15/37] Rename static to rest in docstring Co-authored-by: Yashraj Bhosale --- elastica/joint.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 3bcc690d3..f4dcb129f 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -213,8 +213,8 @@ class FixedJoint(FreeJoint): nut: float Rotational damping coefficient of the joint. static_rotation: np.array - Static 3x3 rotation matrix from system one to system two. - Instead of aligning the directors of both systems directly, a desired rotational offset C_12* is enforced. + 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. """ def __init__(self, k, nu, kt, nut=0.0, use_static_rotation=True): From e3792f78fc8c1f959cf20179d4f9322620443da8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 10:03:57 +0200 Subject: [PATCH 16/37] rename error_rot to dev_rot Co-authored-by: Yashraj Bhosale --- elastica/joint.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/elastica/joint.py b/elastica/joint.py index f4dcb129f..51356be4e 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -270,7 +270,7 @@ def apply_torques(self, system_one, index_one, system_two, index_two): # C_22* is rotation matrix from current orientation of system 2 to desired orientation of system 2 # C_21 is the inverse of C_12, which describes the relative (current) rotation from system 1 to system 2 # C_12* is the desired rotation between systems one and two, which is saved in the static_rotation attribute - error_rot = rel_rot.T @ self.static_rotation + dev_rot = rel_rot.T @ self.rest_rotation_matrix # compute rotation vectors based on C_22* rot_vec = Rotation.from_matrix(error_rot).as_rotvec() From 95f4f8b5f3276238c37b95ee0ec5d21ef0b0bfc7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 10:04:24 +0200 Subject: [PATCH 17/37] Rename error_omega to dev_omega Co-authored-by: Yashraj Bhosale --- elastica/joint.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 51356be4e..11de1e875 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -278,10 +278,11 @@ def apply_torques(self, system_one, index_one, system_two, index_two): # rotate rotation vector into inertial frame rot_vec_inertial_frame = system_two_director.T @ rot_vec - # error in rotation velocity between system 1 and system 2 - error_omega = ( - system_one.omega_collection[..., index_one] - - system_two.omega_collection[..., index_two] + # deviation in rotation velocity between system 1 and system 2 + # first convert to lab frame, then take differences + dev_omega = ( + system_one_director.T @ system_one.omega_collection[..., index_one] + - system_two_director.T @ system_two.omega_collection[..., index_two] ) # we compute the constraining torque using a rotational spring - damper system in the inertial frame From 3b7a1fdbebe2b8fa3c9b1a6430a25defed70e746 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 10:05:59 +0200 Subject: [PATCH 18/37] Finish renaming error_rot to dev_rot and error_omega to dev_omega --- elastica/joint.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 11de1e875..c9a4816f9 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -273,7 +273,7 @@ def apply_torques(self, system_one, index_one, system_two, index_two): dev_rot = rel_rot.T @ self.rest_rotation_matrix # compute rotation vectors based on C_22* - rot_vec = Rotation.from_matrix(error_rot).as_rotvec() + rot_vec = Rotation.from_matrix(dev_rot).as_rotvec() # rotate rotation vector into inertial frame rot_vec_inertial_frame = system_two_director.T @ rot_vec @@ -286,7 +286,7 @@ def apply_torques(self, system_one, index_one, system_two, index_two): ) # we compute the constraining torque using a rotational spring - damper system in the inertial frame - torque = self.kt * rot_vec_inertial_frame - self.nut * error_omega + torque = self.kt * rot_vec_inertial_frame - self.nut * dev_omega # The opposite torques will be applied to system one and two after rotating the torques into the local frame system_one.external_torques[..., index_one] -= system_one_director @ torque From 9a5cf1cb90ebd7fb6a215d799eb27c5d0bd1c5f0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 10:10:00 +0200 Subject: [PATCH 19/37] Some more renaming of variables - `use_static_rotation` --> `enforce_rest_joint_state` - `self.static_rotation` --> `self.rest_rotation_matrix` --- elastica/joint.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index c9a4816f9..29990f37b 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -212,12 +212,12 @@ class FixedJoint(FreeJoint): Rotational stiffness coefficient of the joint. nut: float Rotational damping coefficient of the joint. - static_rotation: np.array + rest_rotation_matrix: np.array 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. """ - def __init__(self, k, nu, kt, nut=0.0, use_static_rotation=True): + def __init__(self, k, nu, kt, nut=0.0, enforce_rest_joint_state=True): """ Parameters @@ -230,8 +230,8 @@ def __init__(self, k, nu, kt, nut=0.0, use_static_rotation=True): Rotational stiffness coefficient of the joint. nut: float = 0. Rotational damping coefficient of the joint. - use_static_rotation: bool - If True, the initial rotation offset between the two systems is recorded + enforce_rest_joint_state: bool + If True, the initial rotation angle between the two systems is recorded and enforced throughout the entire simulation. (default=True) """ super().__init__(k, nu) @@ -242,10 +242,10 @@ def __init__(self, k, nu, kt, nut=0.0, use_static_rotation=True): # if a static rotation offset between the two systems should not be enforced, # we set the relative rotation to the identity matrix. Otherwise - if use_static_rotation: - self.static_rotation = None + if enforce_rest_joint_state: + self.rest_rotation_matrix = None else: - self.static_rotation = np.eye(3) + self.rest_rotation_matrix = np.eye(3) # Apply force is same as free joint def apply_forces(self, rod_one, index_one, rod_two, index_two): @@ -257,9 +257,9 @@ def apply_torques(self, system_one, index_one, system_two, index_two): system_one_director = system_one.director_collection[..., index_one] system_two_director = system_two.director_collection[..., index_two] - if self.static_rotation is None: + if self.rest_rotation_matrix is None: # this if clause should be active during the first timestep for the case use_static_rotation==True - self.static_rotation = system_one_director @ system_two_director.T + self.rest_rotation_matrix = system_one_director @ system_two_director.T # rel_rot: C_12 = C_1I @ C_I2 # C_12 is relative rotation matrix from system 1 to system 2 From cb84464402707586dbc511df29a4156d42a7bdc0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 12:16:38 +0200 Subject: [PATCH 20/37] Make `rest_rotation_matrix` initialization parameter of FixedJoint class We provide a function `get_relative_rotation_two_systems` to easily compute this rest rotation matrix --- elastica/joint.py | 53 ++++++++++++++++++++++++++++++++++++----------- 1 file changed, 41 insertions(+), 12 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 29990f37b..80e73197a 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -214,10 +214,11 @@ class FixedJoint(FreeJoint): Rotational damping coefficient of the joint. rest_rotation_matrix: np.array 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. + Instead of aligning the directors of both systems directly, a desired rest rotational matrix labeled C_12* + is enforced. """ - def __init__(self, k, nu, kt, nut=0.0, enforce_rest_joint_state=True): + def __init__(self, k, nu, kt, nut=0.0, rest_rotation_matrix=np.eye(3)): """ Parameters @@ -230,9 +231,12 @@ def __init__(self, k, nu, kt, nut=0.0, enforce_rest_joint_state=True): Rotational stiffness coefficient of the joint. nut: float = 0. Rotational damping coefficient of the joint. - enforce_rest_joint_state: bool - If True, the initial rotation angle between the two systems is recorded - and enforced throughout the entire simulation. (default=True) + rest_rotation_matrix: np.array + 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=np.eye(3)) """ super().__init__(k, nu) # additional in-plane constraint through restoring torque @@ -240,12 +244,11 @@ def __init__(self, k, nu, kt, nut=0.0, enforce_rest_joint_state=True): self.kt = kt self.nut = nut - # if a static rotation offset between the two systems should not be enforced, - # we set the relative rotation to the identity matrix. Otherwise - if enforce_rest_joint_state: - self.rest_rotation_matrix = None - else: - self.rest_rotation_matrix = np.eye(3) + # TODO: compute the rest rotation matrix directly during initialization + # as soon as systems (e.g. `rod_one` and `rod_two`) and indices (e.g. `index_one` and `index_two`) + # are available in the __init__ + assert rest_rotation_matrix.shape == (3, 3), "Rest rotation matrix must be 3x3" + self.rest_rotation_matrix = rest_rotation_matrix # Apply force is same as free joint def apply_forces(self, rod_one, index_one, rod_two, index_two): @@ -278,7 +281,7 @@ def apply_torques(self, system_one, index_one, system_two, index_two): # rotate rotation vector into inertial frame rot_vec_inertial_frame = system_two_director.T @ rot_vec - # deviation in rotation velocity between system 1 and system 2 + # deviation in rotation velocity between system 1 and system 2 # first convert to lab frame, then take differences dev_omega = ( system_one_director.T @ system_one.omega_collection[..., index_one] @@ -293,6 +296,32 @@ def apply_torques(self, system_one, index_one, system_two, index_two): system_two.external_torques[..., index_two] += system_two_director @ torque +def get_relative_rotation_two_systems(system_one, index_one, system_two, index_two): + """ + Compute the relative rotation matrix C_12 between system one and system two at the specified nodes. + + Parameters + ---------- + rod_one : object + Rod-like object + index_one : int + Index of first rod for joint. + rod_two : object + Rod-like object + index_two : int + Index of second rod for joint. + + Returns + ------- + relative_rotation_matrix : np.array + Relative rotation matrix C_12 between the two systems for their current state. + """ + return ( + system_one.director_collection[..., index_one] + @ system_two.director_collection[..., index_two] + ) + + @numba.njit(cache=True) def _dot_product(a, b): sum = 0.0 From 63c83996e3c6de7078ce5e148c8beea0857bda1e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 12:18:15 +0200 Subject: [PATCH 21/37] Fix utilization of diagnostic directors data in `fixed_joint_torsion` --- examples/JointCases/fixed_joint_torsion.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/JointCases/fixed_joint_torsion.py b/examples/JointCases/fixed_joint_torsion.py index 00d271ea3..6e24d4e9f 100644 --- a/examples/JointCases/fixed_joint_torsion.py +++ b/examples/JointCases/fixed_joint_torsion.py @@ -124,12 +124,12 @@ class FixedJointSimulator( plot_orientation( "Orientation of last node of rod 1", pp_list_rod1["time"], - np.array(pp_list_rod1["director"])[..., -1], + np.array(pp_list_rod1["directors"])[..., -1], ) plot_orientation( "Orientation of last node of rod 2", pp_list_rod2["time"], - np.array(pp_list_rod2["director"])[..., -1], + np.array(pp_list_rod2["directors"])[..., -1], ) PLOT_FIGURE = True From 8fd8976465a320976915979d24d84020641f643e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 12:25:14 +0200 Subject: [PATCH 22/37] Remove initialization of `rest_rotation_matrix` during main loop --- elastica/joint.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 80e73197a..ca7f50169 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -260,10 +260,6 @@ def apply_torques(self, system_one, index_one, system_two, index_two): system_one_director = system_one.director_collection[..., index_one] system_two_director = system_two.director_collection[..., index_two] - if self.rest_rotation_matrix is None: - # this if clause should be active during the first timestep for the case use_static_rotation==True - self.rest_rotation_matrix = system_one_director @ system_two_director.T - # rel_rot: C_12 = C_1I @ C_I2 # C_12 is relative rotation matrix from system 1 to system 2 # C_1I is the rotation from system 1 to the inertial frame (i.e. the world frame) From c97c9b4c5d777d4c131f8cffbf8a5ee012ddbdce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 19:54:59 +0200 Subject: [PATCH 23/37] Update elastica/joint.py Co-authored-by: Arman Tekinalp <53585636+armantekinalp@users.noreply.github.com> --- elastica/joint.py | 1 + 1 file changed, 1 insertion(+) diff --git a/elastica/joint.py b/elastica/joint.py index ca7f50169..0372c4d6c 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -232,6 +232,7 @@ def __init__(self, k, nu, kt, nut=0.0, rest_rotation_matrix=np.eye(3)): nut: float = 0. Rotational damping coefficient of the joint. rest_rotation_matrix: np.array + 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, From dc59e2978dd3ef47bcf7f1326a775d324de7e488 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 19:55:10 +0200 Subject: [PATCH 24/37] Update elastica/joint.py Co-authored-by: Arman Tekinalp <53585636+armantekinalp@users.noreply.github.com> --- elastica/joint.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/elastica/joint.py b/elastica/joint.py index 0372c4d6c..8938c7004 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -295,7 +295,7 @@ def apply_torques(self, system_one, index_one, system_two, index_two): def get_relative_rotation_two_systems(system_one, index_one, system_two, index_two): """ - Compute the relative rotation matrix C_12 between system one and system two at the specified nodes. + Compute the relative rotation matrix C_12 between system one and system two at the specified elements. Parameters ---------- From 91d18173b492499cd574b0d13ca9b7a94dace42c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 21:53:12 +0200 Subject: [PATCH 25/37] Replace numpy default argument with None --- elastica/joint.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index ca7f50169..5cc6745d0 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -218,7 +218,7 @@ class FixedJoint(FreeJoint): is enforced. """ - def __init__(self, k, nu, kt, nut=0.0, rest_rotation_matrix=np.eye(3)): + def __init__(self, k, nu, kt, nut=0.0, rest_rotation_matrix=None): """ Parameters @@ -236,7 +236,7 @@ def __init__(self, k, nu, kt, nut=0.0, rest_rotation_matrix=np.eye(3)): 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=np.eye(3)) + (default=None) """ super().__init__(k, nu) # additional in-plane constraint through restoring torque @@ -247,6 +247,8 @@ def __init__(self, k, nu, kt, nut=0.0, rest_rotation_matrix=np.eye(3)): # TODO: compute the rest rotation matrix directly during initialization # as soon as systems (e.g. `rod_one` and `rod_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 From 7b351c5abf45cde2b77375fdc533fe1e5393d5f4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 21:58:09 +0200 Subject: [PATCH 26/37] Fix sign error of dev_omega --- elastica/joint.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 5cc6745d0..561019857 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -280,10 +280,10 @@ def apply_torques(self, system_one, index_one, system_two, index_two): rot_vec_inertial_frame = system_two_director.T @ rot_vec # deviation in rotation velocity between system 1 and system 2 - # first convert to lab frame, then take differences + # first convert to inertial frame, then take differences dev_omega = ( - system_one_director.T @ system_one.omega_collection[..., index_one] - - system_two_director.T @ system_two.omega_collection[..., index_two] + system_two_director.T @ system_two.omega_collection[..., index_two] + - system_one_director.T @ system_one.omega_collection[..., index_one] ) # we compute the constraining torque using a rotational spring - damper system in the inertial frame From e334b09ac3a8c299fa6a8bf057b3164a5cb45295 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 21:58:35 +0200 Subject: [PATCH 27/37] Also rotate omega to inertial frame in test_fixedjoint --- tests/test_joint.py | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/tests/test_joint.py b/tests/test_joint.py index c215c2669..31269b12d 100644 --- a/tests/test_joint.py +++ b/tests/test_joint.py @@ -294,7 +294,7 @@ def test_fixedjoint(): kt = 1e6 # Positional and rotational damping between systems nu = 1 - nut = 1e1 + nut = 1e2 # Rod indexes rod1_index = -1 @@ -317,7 +317,7 @@ def test_fixedjoint(): dampingforce = nu * normal_relative_vel * distance / end_distance contactforce = elasticforce - dampingforce - fxjt = FixedJoint(k, nu, kt, nut, use_static_rotation=False) + fxjt = FixedJoint(k, nu, kt, nut, rest_rotation_matrix=np.eye(3)) fxjt.apply_forces(rod1, rod1_index, rod2, rod2_index) fxjt.apply_torques(rod1, rod1_index, rod2, rod2_index) @@ -338,28 +338,30 @@ def test_fixedjoint(): error_rot = rod1_director @ rod2_director.T # compute rotation vector from system one to system two based on relative rotation matrix - rot_vec = Rotation.from_matrix(error_rot).as_rotvec() + rot_vec = Rotation.from_matrix(error_rot.T).as_rotvec() # rotate rotation vector into inertial frame - rot_vec = rod1_director.T @ rot_vec + rot_vec = rod2_director.T @ rot_vec - # error in rotation velocity between system 1 and system 2 - error_omega = ( - rod2.omega_collection[..., rod2_index] - rod1.omega_collection[..., rod1_index] + # deviation in rotation velocity between system 1 and system 2 + # first convert to inertial frame, then take differences + dev_omega = ( + rod2_director.T @ rod2.omega_collection[..., rod2_index] + - rod1_director.T @ rod1.omega_collection[..., rod1_index] ) # we compute the constraining torque using a rotational spring - damper system in the inertial frame - torque = kt * rot_vec - nut * error_omega + torque = kt * rot_vec - nut * dev_omega # The opposite torques will be applied to system one and two after rotating the torques into the local frame torque_rod1 = rod1_director @ torque torque_rod2 = rod2_director @ torque assert_allclose( - rod1.external_torques[..., rod1_index], torque_rod1, atol=Tolerance.atol() + rod1.external_torques[..., rod1_index], -torque_rod1, atol=Tolerance.atol() ) assert_allclose( - rod2.external_torques[..., rod2_index], -torque_rod2, atol=Tolerance.atol() + rod2.external_torques[..., rod2_index], torque_rod2, atol=Tolerance.atol() ) From 9d4869737a4387f26e66739db128c04d147313b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 21:59:31 +0200 Subject: [PATCH 28/37] Reformatting with black --- tests/test_joint.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_joint.py b/tests/test_joint.py index 31269b12d..e7a75f9b7 100644 --- a/tests/test_joint.py +++ b/tests/test_joint.py @@ -346,8 +346,8 @@ def test_fixedjoint(): # deviation in rotation velocity between system 1 and system 2 # first convert to inertial frame, then take differences dev_omega = ( - rod2_director.T @ rod2.omega_collection[..., rod2_index] - - rod1_director.T @ rod1.omega_collection[..., rod1_index] + rod2_director.T @ rod2.omega_collection[..., rod2_index] + - rod1_director.T @ rod1.omega_collection[..., rod1_index] ) # we compute the constraining torque using a rotational spring - damper system in the inertial frame From 6d360f6352393a911ce0aa7a4425defd1fa5dee4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 22:28:26 +0200 Subject: [PATCH 29/37] Remove `joint_cases_callback` --- examples/JointCases/joint_cases_callback.py | 22 --------------------- 1 file changed, 22 deletions(-) delete mode 100644 examples/JointCases/joint_cases_callback.py diff --git a/examples/JointCases/joint_cases_callback.py b/examples/JointCases/joint_cases_callback.py deleted file mode 100644 index 8cf4e90c4..000000000 --- a/examples/JointCases/joint_cases_callback.py +++ /dev/null @@ -1,22 +0,0 @@ -from elastica.callback_functions import CallBackBaseClass - - -class JointCasesCallback(CallBackBaseClass): - """ - Call back function for testing joints - """ - - def __init__(self, step_skip: int, callback_params: dict): - CallBackBaseClass.__init__(self) - self.every = step_skip - self.callback_params = callback_params - - def make_callback(self, system, time, current_step: int): - if current_step % self.every == 0: - self.callback_params["time"].append(time) - self.callback_params["step"].append(current_step) - self.callback_params["position"].append(system.position_collection.copy()) - self.callback_params["director"].append(system.director_collection.copy()) - self.callback_params["velocity"].append(system.velocity_collection.copy()) - - return From d31173e716ae4e575075360274f437cd752a6bf6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 22:34:39 +0200 Subject: [PATCH 30/37] Fix missing transpose in `get_relative_rotation_two_systems` --- elastica/joint.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/elastica/joint.py b/elastica/joint.py index be7b71b76..f4014d6ef 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -317,7 +317,7 @@ def get_relative_rotation_two_systems(system_one, index_one, system_two, index_t """ return ( system_one.director_collection[..., index_one] - @ system_two.director_collection[..., index_two] + @ system_two.director_collection[..., index_two].T ) From f1574138447a9ed4d67dc74610866760f2cfb236 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 22:39:24 +0200 Subject: [PATCH 31/37] Align rod2 in `fixed_joint_torsion` along (0,1,0), enabling example to use `rest_rotation_matrix` --- examples/JointCases/fixed_joint_torsion.py | 27 ++++++++++++++-------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/examples/JointCases/fixed_joint_torsion.py b/examples/JointCases/fixed_joint_torsion.py index 6e24d4e9f..fb13fa5cb 100644 --- a/examples/JointCases/fixed_joint_torsion.py +++ b/examples/JointCases/fixed_joint_torsion.py @@ -7,6 +7,7 @@ # FIXME without appending sys.path make it more generic sys.path.append("../../") from elastica import * +from elastica.joint import get_relative_rotation_two_systems from examples.JointCases.joint_cases_postprocessing import ( plot_position, plot_orientation, @@ -26,9 +27,10 @@ class 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) +direction_rod1 = np.array([0.0, 0.0, 1.0]) +normal_rod1 = np.array([0.0, 1.0, 0.0]) +direction_rod2 = np.array([0.0, 1.0, 0.0]) +normal_rod2 = np.array([0.0, 0.0, 1.0]) base_length = 0.2 base_radius = 0.007 base_area = np.pi * base_radius ** 2 @@ -38,14 +40,14 @@ class FixedJointSimulator( shear_modulus = E / (poisson_ratio + 1.0) start_rod_1 = np.zeros((3,)) -start_rod_2 = start_rod_1 + direction * base_length +start_rod_2 = start_rod_1 + direction_rod1 * base_length # Create rod 1 rod1 = CosseratRod.straight_rod( n_elem, start_rod_1, - direction, - normal, + direction_rod1, + normal_rod1, base_length, base_radius, density, @@ -58,8 +60,8 @@ class FixedJointSimulator( rod2 = CosseratRod.straight_rod( n_elem, start_rod_2, - direction, - normal, + direction_rod2, + normal_rod2, base_length, base_radius, density, @@ -77,7 +79,14 @@ 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=1.0, kt=1e3, nut=1e-3) +).using( + FixedJoint, + k=1e5, + nu=1.0, + kt=1e3, + nut=1e-3, + rest_rotation_matrix=get_relative_rotation_two_systems(rod1, -1, rod2, 0), +) # Add forces to rod2 fixed_joint_sim.add_forcing_to(rod2).using( From cf368beb225d3722934b9efe380dc35524076cbc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 22:42:06 +0200 Subject: [PATCH 32/37] Add example to docstring of get_relative_rotation_two_systems Co-authored-by: Seung Hyun Kim --- elastica/joint.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/elastica/joint.py b/elastica/joint.py index f4014d6ef..5954351bd 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -299,6 +299,16 @@ def get_relative_rotation_two_systems(system_one, index_one, system_two, index_t """ Compute the relative rotation matrix C_12 between system one and system two at the specified elements. + Examples + ---------- + + >>> init_rot = get_relative_rotation_two_systems(...) + >>> ... + + See Also + --------- + FixedJoint + Parameters ---------- rod_one : object From 4868dc51e519f082fbd1b35bd4cbb7b9d45e23bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 22:50:21 +0200 Subject: [PATCH 33/37] Finish examples of `get_relative_rotation_two_systems` --- elastica/joint.py | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 5954351bd..35a54215d 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -301,9 +301,20 @@ def get_relative_rotation_two_systems(system_one, index_one, system_two, index_t Examples ---------- - - >>> init_rot = get_relative_rotation_two_systems(...) - >>> ... + How to get the relative rotation between two systems (e.g. the rotation from end of rod one to base of rod two): + + >>> rel_rot_mat = get_relative_rotation_two_systems(rod1, -1, rod2, 0) + + How to initialize a FixedJoint with a rest rotation between the two systems, + which is enforced throughout the simulation: + + >>> simulator.connect( + ... first_rod=rod1, second_rod=rod2, first_connect_idx=-1, second_connect_idx=0 + ... ).using( + ... FixedJoint, + ... ku=1e6, nu=0.0, kt=1e3, nut=0.0, + ... rest_rotation_matrix=get_relative_rotation_two_systems(rod1, -1, rod2, 0) + ... ) See Also --------- From f36cf35ca05562b5ac99f897b5bf6ce02057a738 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sat, 16 Jul 2022 22:50:38 +0200 Subject: [PATCH 34/37] Some reformatting --- elastica/joint.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index 35a54215d..b68ae2186 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -315,10 +315,10 @@ def get_relative_rotation_two_systems(system_one, index_one, system_two, index_t ... ku=1e6, nu=0.0, kt=1e3, nut=0.0, ... rest_rotation_matrix=get_relative_rotation_two_systems(rod1, -1, rod2, 0) ... ) - + See Also --------- - FixedJoint + FixedJoint Parameters ---------- From 9397d0095920e9f5885f2308557a0edd6905f02e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sun, 17 Jul 2022 09:50:34 +0200 Subject: [PATCH 35/37] Update elastica/joint.py Co-authored-by: Arman Tekinalp <53585636+armantekinalp@users.noreply.github.com> --- elastica/joint.py | 1 + 1 file changed, 1 insertion(+) diff --git a/elastica/joint.py b/elastica/joint.py index b68ae2186..bd0fbba22 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -213,6 +213,7 @@ class FixedJoint(FreeJoint): nut: float Rotational damping coefficient of the joint. rest_rotation_matrix: np.array + 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. From 035e0ed52f9408a67ccadc1bb7db06b766b2a584 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sun, 17 Jul 2022 21:34:30 +0200 Subject: [PATCH 36/37] Replace scipy rotation vector with custom numba _inv_rotate --- elastica/joint.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/elastica/joint.py b/elastica/joint.py index b68ae2186..a20457c27 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -4,8 +4,8 @@ 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 math import sqrt -from scipy.spatial.transform import Rotation class FreeJoint: @@ -275,7 +275,12 @@ def apply_torques(self, system_one, index_one, system_two, index_two): dev_rot = rel_rot.T @ self.rest_rotation_matrix # compute rotation vectors based on C_22* - rot_vec = Rotation.from_matrix(dev_rot).as_rotvec() + # scipy implementation + # rot_vec = Rotation.from_matrix(dev_rot).as_rotvec() + # + # implementation using custom _inv_rotate compiled with numba + # rotation vector between identity matrix and C_22* + rot_vec = _inv_rotate(np.dstack([np.eye(3), dev_rot.T])).squeeze() # rotate rotation vector into inertial frame rot_vec_inertial_frame = system_two_director.T @ rot_vec From d84f5f1234aad142f732c15908ba7e40dd2eb466 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maximilian=20St=C3=B6lzle?= Date: Sun, 17 Jul 2022 22:26:37 +0200 Subject: [PATCH 37/37] Test fixed joint for a variety of rest rotation matrices --- tests/test_joint.py | 49 +++++++++++++++++++++++++++++++++------------ 1 file changed, 36 insertions(+), 13 deletions(-) diff --git a/tests/test_joint.py b/tests/test_joint.py index e7a75f9b7..7c56e7372 100644 --- a/tests/test_joint.py +++ b/tests/test_joint.py @@ -6,8 +6,9 @@ from numpy.testing import assert_allclose from elastica.utils import Tolerance from elastica.rod.cosserat_rod import CosseratRod -import importlib import elastica +import importlib +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 @@ -224,9 +225,19 @@ def test_hingejoint(): ) -def test_fixedjoint(): +rest_euler_angles = [ + np.array([0.0, 0.0, 0.0]), + 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), +] + + +@pytest.mark.parametrize("rest_euler_angle", rest_euler_angles) +def test_fixedjoint(rest_euler_angle): # Define the rod for testing - # Some rod properties. We need them for constructer, they are not used. + # Some rod properties. We need them for constructor, they are not used. normal1 = np.array([0.0, 1.0, 0.0]) direction = np.array([1.0, 0.0, 0.0]) normal2 = np.array([0.0, 0.0, 1.0]) @@ -317,7 +328,10 @@ def test_fixedjoint(): dampingforce = nu * normal_relative_vel * distance / end_distance contactforce = elasticforce - dampingforce - fxjt = FixedJoint(k, nu, kt, nut, rest_rotation_matrix=np.eye(3)) + rest_rotation_matrix = Rotation.from_euler( + "xyz", rest_euler_angle, degrees=False + ).as_matrix() + fxjt = FixedJoint(k, nu, kt, nut, rest_rotation_matrix=rest_rotation_matrix) fxjt.apply_forces(rod1, rod1_index, rod2, rod2_index) fxjt.apply_torques(rod1, rod1_index, rod2, rod2_index) @@ -334,14 +348,23 @@ def test_fixedjoint(): rod1_director = rod1.director_collection[..., rod1_index] rod2_director = rod2.director_collection[..., rod2_index] - # relative rotation matrix from system 1 to system 2: C_12 = C_1W @ C_W2 where W denotes the inertial frame - error_rot = rod1_director @ rod2_director.T - - # compute rotation vector from system one to system two based on relative rotation matrix - rot_vec = Rotation.from_matrix(error_rot.T).as_rotvec() + # rel_rot: C_12 = C_1I @ C_I2 + # C_12 is relative rotation matrix from system 1 to system 2 + # C_1I is the rotation from system 1 to the inertial frame (i.e. the world frame) + # C_I2 is the rotation from the inertial frame to system 2 frame (inverse of system_two_director) + rel_rot = rod1_director @ rod2_director.T + # error_rot: C_22* = C_21 @ C_12* + # C_22* is rotation matrix from current orientation of system 2 to desired orientation of system 2 + # C_21 is the inverse of C_12, which describes the relative (current) rotation from system 1 to system 2 + # C_12* is the desired rotation between systems one and two, which is saved in the static_rotation attribute + dev_rot = rel_rot.T @ rest_rotation_matrix + + # compute rotation vectors based on C_22* + # scipy implementation + rot_vec = Rotation.from_matrix(dev_rot).as_rotvec() # rotate rotation vector into inertial frame - rot_vec = rod2_director.T @ rot_vec + rot_vec_inertial_frame = rod2_director.T @ rot_vec # deviation in rotation velocity between system 1 and system 2 # first convert to inertial frame, then take differences @@ -351,14 +374,14 @@ def test_fixedjoint(): ) # we compute the constraining torque using a rotational spring - damper system in the inertial frame - torque = kt * rot_vec - nut * dev_omega + torque = kt * rot_vec_inertial_frame - nut * dev_omega # The opposite torques will be applied to system one and two after rotating the torques into the local frame - torque_rod1 = rod1_director @ torque + torque_rod1 = -rod1_director @ torque torque_rod2 = rod2_director @ torque assert_allclose( - rod1.external_torques[..., rod1_index], -torque_rod1, atol=Tolerance.atol() + rod1.external_torques[..., rod1_index], torque_rod1, atol=Tolerance.atol() ) assert_allclose( rod2.external_torques[..., rod2_index], torque_rod2, atol=Tolerance.atol()