Repository: GazzolaLab/PyElastica

Issue:
Cross-check and modify if needed joint damping force direction
As pointed out in PR #149 damping force direction sign should be opposite 

https://github.com/GazzolaLab/PyElastica/blob/d0cb959ab6ac328fd518fa846f8edf2ea164992f/elastica/joint.py#L92

Correct implementation should be

```python
 damping_force = self.nu * normal_relative_velocity 
```

Discussion:

Patch:
diff --git a/elastica/experimental/connection_contact_joint/parallel_connection.py b/elastica/experimental/connection_contact_joint/parallel_connection.py
index 5113e07..0defeb0 100644
--- a/elastica/experimental/connection_contact_joint/parallel_connection.py
+++ b/elastica/experimental/connection_contact_joint/parallel_connection.py
@@ -1,17 +1,12 @@
 __doc__ = """Contains SurfaceJointSideBySide class which connects two parallel rods ."""
 import numpy as np
-import numba
 from numba import njit
 from elastica.joint import FreeJoint
 
 # Join the two rods
 from elastica._linalg import (
     _batch_norm,
-    _batch_cross,
     _batch_matvec,
-    _batch_dot,
-    _batch_matmul,
-    _batch_matrix_transpose,
 )
 
 
@@ -203,28 +198,8 @@ class SurfaceJointSideBySide(FreeJoint):
             rod_two_velocity_collection[:, index_two]
             + rod_two_velocity_collection[:, index_two + 1]
         )
-
         relative_velocity = rod_two_element_velocity - rod_one_element_velocity
-
-        distance = np.linalg.norm(distance_vector)
-
-        # normalized_distance_vector = np.zeros((relative_velocity.shape))
-        #
-        # idx_nonzero_distance = np.where(distance >= 1e-12)[0]
-
-        if distance >= 1e-12:
-            normalized_distance_vector = distance_vector / distance
-        else:
-            normalized_distance_vector = np.zeros(
-                3,
-            )
-
-        normal_relative_velocity_vector = (
-            np.dot(relative_velocity, normalized_distance_vector)
-            * normalized_distance_vector
-        )
-
-        damping_force = -nu * normal_relative_velocity_vector
+        damping_force = nu * relative_velocity
 
         # Compute the total force
         total_force = spring_force + damping_force
diff --git a/elastica/joint.py b/elastica/joint.py
index 4a11760..b7e258e 100644
--- a/elastica/joint.py
+++ b/elastica/joint.py
@@ -2,8 +2,6 @@ __doc__ = """ Module containing joint classes to connect multiple rods together.
 __all__ = ["FreeJoint", "HingeJoint", "FixedJoint", "ExternalContact", "SelfContact"]
 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
@@ -68,32 +66,15 @@ class FreeJoint:
             rod_two.position_collection[..., index_two]
             - rod_one.position_collection[..., index_one]
         )
-        # Calculate norm of end_distance_vector
-        # this implementation timed: 2.48 µs ± 126 ns per loop (mean ± std. dev. of 7 runs, 100000 loops each)
-        end_distance = np.sqrt(np.dot(end_distance_vector, end_distance_vector))
-
-        # Below if check is not efficient find something else
-        # We are checking if end of rod1 and start of rod2 are at the same point in space
-        # If they are at the same point in space, it is a zero vector.
-        if end_distance <= Tolerance.atol():
-            normalized_end_distance_vector = np.array([0.0, 0.0, 0.0])
-        else:
-            normalized_end_distance_vector = end_distance_vector / end_distance
-
         elastic_force = self.k * end_distance_vector
 
         relative_velocity = (
             rod_two.velocity_collection[..., index_two]
             - rod_one.velocity_collection[..., index_one]
         )
-        normal_relative_velocity = (
-            np.dot(relative_velocity, normalized_end_distance_vector)
-            * normalized_end_distance_vector
-        )
-        damping_force = -self.nu * normal_relative_velocity
+        damping_force = self.nu * relative_velocity
 
         contact_force = elastic_force + damping_force
-
         rod_one.external_forces[..., index_one] += contact_force
         rod_two.external_forces[..., index_two] -= contact_force
 
diff --git a/examples/JointCases/experimental/generic_system_type_fixed_joint.py b/examples/ExperimentalCases/GenericSystemConnectionCases/generic_system_type_fixed_joint.py
similarity index 98%
rename from examples/JointCases/experimental/generic_system_type_fixed_joint.py
rename to examples/ExperimentalCases/GenericSystemConnectionCases/generic_system_type_fixed_joint.py
index 551e365..d338b2a 100644
--- a/examples/JointCases/experimental/generic_system_type_fixed_joint.py
+++ b/examples/ExperimentalCases/GenericSystemConnectionCases/generic_system_type_fixed_joint.py
@@ -5,7 +5,7 @@ from elastica import *
 from elastica.experimental.connection_contact_joint.generic_system_type_connection import (
     GenericSystemTypeFixedJoint,
 )
-from examples.JointCases.joint_cases_postprocessing import (
+from joint_cases_postprocessing import (
     plot_position,
     plot_video,
     plot_video_xy,
diff --git a/examples/JointCases/experimental/generic_system_type_spherical_joint.py b/examples/ExperimentalCases/GenericSystemConnectionCases/generic_system_type_spherical_joint.py
similarity index 97%
rename from examples/JointCases/experimental/generic_system_type_spherical_joint.py
rename to examples/ExperimentalCases/GenericSystemConnectionCases/generic_system_type_spherical_joint.py
index 7d447b5..3d3a61d 100644
--- a/examples/JointCases/experimental/generic_system_type_spherical_joint.py
+++ b/examples/ExperimentalCases/GenericSystemConnectionCases/generic_system_type_spherical_joint.py
@@ -1,4 +1,4 @@
-__doc__ = """Spherical(Free) joint example, for detailed explanation refer to Zhang et. al. Nature Comm.  
+__doc__ = """Spherical(Free) joint example, for detailed explanation refer to Zhang et. al. Nature Comm.
 methods section."""
 
 import numpy as np
@@ -6,7 +6,7 @@ from elastica import *
 from elastica.experimental.connection_contact_joint.generic_system_type_connection import (
     GenericSystemTypeFreeJoint,
 )
-from examples.JointCases.joint_cases_postprocessing import (
+from joint_cases_postprocessing import (
     plot_position,
     plot_video,
     plot_video_xy,
diff --git a/examples/ExperimentalCases/GenericSystemConnectionCases/joint_cases_postprocessing.py b/examples/ExperimentalCases/GenericSystemConnectionCases/joint_cases_postprocessing.py
new file mode 100644
index 0000000..c1ca4dd
--- /dev/null
+++ b/examples/ExperimentalCases/GenericSystemConnectionCases/joint_cases_postprocessing.py
@@ -0,0 +1,335 @@
+import numpy as np
+from matplotlib import pyplot as plt
+from matplotlib.colors import to_rgb
+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,
+    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)
+
+    ax.grid(b=True, which="minor", color="k", linestyle="--")
+    ax.grid(b=True, which="major", color="k", linestyle="-")
+    ax.plot(position_of_rod1[:, 0, -1], position_of_rod1[:, 1, -1], "r-", label="rod1")
+    ax.plot(
+        position_of_rod2[:, 0, -1],
+        position_of_rod2[:, 1, -1],
+        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()
+
+    if SAVE_FIGURE:
+        fig.savefig(filename)
+
+
+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()
+        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,
+    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"]
+    metadata = dict(title="Movie Test", artist="Matplotlib", comment="Movie support!")
+    writer = FFMpegWriter(fps=fps, metadata=metadata)
+    fig = plt.figure(figsize=(10, 8), frameon=True, dpi=150)
+    with writer.saving(fig, video_name, 100):
+        for time in range(1, len(time)):
+            fig.clf()
+            ax = plt.axes(projection="3d")  # fig.add_subplot(111)
+            ax.grid(b=True, which="minor", color="k", linestyle="--")
+            ax.grid(b=True, which="major", color="k", linestyle="-")
+            ax.plot(
+                position_of_rod1[time, 0],
+                position_of_rod1[time, 1],
+                position_of_rod1[time, 2],
+                "or",
+                label="rod1",
+            )
+            ax.plot(
+                position_of_rod2[time, 0],
+                position_of_rod2[time, 1],
+                position_of_rod2[time, 2],
+                "o",
+                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.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,
+    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"]
+    metadata = dict(title="Movie Test", artist="Matplotlib", comment="Movie support!")
+    writer = FFMpegWriter(fps=fps, metadata=metadata)
+    fig = plt.figure()
+    plt.axis("equal")
+    with writer.saving(fig, video_name, 100):
+        for time in range(1, len(time)):
+            fig.clf()
+            plt.plot(
+                position_of_rod1[time, 0], position_of_rod1[time, 1], "or", label="rod1"
+            )
+            plt.plot(
+                position_of_rod2[time, 0],
+                position_of_rod2[time, 1],
+                "o",
+                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,
+    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"]
+    metadata = dict(title="Movie Test", artist="Matplotlib", comment="Movie support!")
+    writer = FFMpegWriter(fps=fps, metadata=metadata)
+    fig = plt.figure()
+    plt.axis("equal")
+    with writer.saving(fig, video_name, 100):
+        for time in range(1, len(time)):
+            fig.clf()
+            plt.plot(
+                position_of_rod1[time, 0], position_of_rod1[time, 2], "or", label="rod1"
+            )
+            plt.plot(
+                position_of_rod2[time, 0],
+                position_of_rod2[time, 2],
+                "o",
+                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.61])
+            plt.xlabel("x [m]")
+            plt.ylabel("z [m]")
+            writer.grab_frame()
diff --git a/examples/JointCases/fixed_joint.py b/examples/JointCases/fixed_joint.py
index 46a7e66..c5c8064 100644
--- a/examples/JointCases/fixed_joint.py
+++ b/examples/JointCases/fixed_joint.py
@@ -31,12 +31,6 @@ 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
 
@@ -77,13 +71,7 @@ fixed_joint_sim.constrain(rod1).using(
 # 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=0e0,
-    kt=1e1,
-    nut=0e0,
-)
+).using(FixedJoint, k=1e5, nu=0.0, kt=1e1, nut=0.0)
 
 # Add forces to rod2
 fixed_joint_sim.add_forcing_to(rod2).using(
@@ -100,6 +88,7 @@ fixed_joint_sim.add_forcing_to(rod2).using(
 # 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,
@@ -115,56 +104,37 @@ pp_list_rod1 = defaultdict(list)
 pp_list_rod2 = defaultdict(list)
 
 fixed_joint_sim.collect_diagnostics(rod1).using(
-    MyCallBack, step_skip=step_skip, callback_params=pp_list_rod1
+    MyCallBack, step_skip=1000, callback_params=pp_list_rod1
 )
 fixed_joint_sim.collect_diagnostics(rod2).using(
-    MyCallBack, step_skip=step_skip, callback_params=pp_list_rod2
+    MyCallBack, step_skip=1000, 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 = True
+SAVE_FIGURE = False
 PLOT_VIDEO = True
 
 # plotting results
 if PLOT_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,
-    )
+    filename = "fixed_joint_test.png"
+    plot_position(pp_list_rod1, pp_list_rod2, filename, SAVE_FIGURE)
 
 if PLOT_VIDEO:
-    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,
-    )
+    filename = "fixed_joint_test.mp4"
+    plot_video(pp_list_rod1, pp_list_rod2, video_name=filename, margin=0.2, fps=100)
     plot_video_xy(
-        plot_params_rod1=pp_list_rod1,
-        plot_params_rod2=pp_list_rod2,
-        plot_params_cylinder=None,
-        video_name=filename + "_xy.mp4",
-        fps=fps,
+        pp_list_rod1, pp_list_rod2, video_name=filename + "_xy.mp4", margin=0.2, fps=100
     )
     plot_video_xz(
-        plot_params_rod1=pp_list_rod1,
-        plot_params_rod2=pp_list_rod2,
-        plot_params_cylinder=None,
-        video_name=filename + "_xz.mp4",
-        fps=fps,
+        pp_list_rod1, pp_list_rod2, video_name=filename + "_xz.mp4", margin=0.2, fps=100
     )
diff --git a/examples/JointCases/fixed_joint_torsion.py b/examples/JointCases/fixed_joint_torsion.py
index bd08f9b..35f9f7f 100644
--- a/examples/JointCases/fixed_joint_torsion.py
+++ b/examples/JointCases/fixed_joint_torsion.py
@@ -119,7 +119,6 @@ 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)
@@ -142,36 +141,30 @@ PLOT_VIDEO = True
 
 # plotting results
 if PLOT_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,
-    )
+    filename = "fixed_joint_torsion_test.png"
+    plot_position(pp_list_rod1, pp_list_rod2, filename, SAVE_FIGURE)
 
 if PLOT_VIDEO:
-    filename = "fixed_joint_torsion_example"
+    filename = "fixed_joint_torsion_test"
     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 226394e..9a16580 100644
--- a/examples/JointCases/hinge_joint.py
+++ b/examples/JointCases/hinge_joint.py
@@ -128,35 +128,15 @@ PLOT_VIDEO = True
 
 # plotting results
 if PLOT_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,
-    )
+    filename = "hinge_joint_test.png"
+    plot_position(pp_list_rod1, pp_list_rod2, filename, SAVE_FIGURE)
 
 if PLOT_VIDEO:
-    filename = "hinge_joint_example"
-    plot_video(
-        pp_list_rod1,
-        pp_list_rod2,
-        plot_params_cylinder=None,
-        video_name=filename + ".mp4",
-        fps=100,
-    )
+    filename = "hinge_joint_test.mp4"
+    plot_video(pp_list_rod1, pp_list_rod2, video_name=filename, margin=0.2, fps=100)
     plot_video_xy(
-        pp_list_rod1,
-        pp_list_rod2,
-        plot_params_cylinder=None,
-        video_name=filename + "_xy.mp4",
-        fps=100,
+        pp_list_rod1, pp_list_rod2, video_name=filename + "_xy.mp4", margin=0.2, fps=100
     )
     plot_video_xz(
-        pp_list_rod1,
-        pp_list_rod2,
-        plot_params_cylinder=None,
-        video_name=filename + "_xz.mp4",
-        fps=100,
+        pp_list_rod1, pp_list_rod2, video_name=filename + "_xz.mp4", margin=0.2, fps=100
     )
diff --git a/examples/JointCases/joint_cases_postprocessing.py b/examples/JointCases/joint_cases_postprocessing.py
index c1ca4dd..d7ea143 100644
--- a/examples/JointCases/joint_cases_postprocessing.py
+++ b/examples/JointCases/joint_cases_postprocessing.py
@@ -4,24 +4,16 @@ from matplotlib.colors import to_rgb
 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,
-    plot_params_cylinder: dict = None,
-    filename="joint_cases_last_node_pos_xy.png",
+    filename="spherical_joint_test.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)
@@ -35,17 +27,8 @@ 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()
 
@@ -54,9 +37,6 @@ 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()
@@ -78,26 +58,15 @@ def plot_orientation(title, time, directors):
 def plot_video(
     plot_params_rod1: dict,
     plot_params_rod2: dict,
-    plot_params_cylinder: dict = None,
-    video_name="joint_cases_video.mp4",
-    fps=100,
-    cylinder: Cylinder = None,
+    video_name="video.mp4",
+    margin=0.2,
+    fps=15,
 ):  # (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"]
@@ -125,73 +94,25 @@ 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.61)
-            ax.set_xlabel("x [m]")
-            ax.set_ylabel("y [m]")
-            ax.set_zlabel("z [m]")
+            ax.set_zlim(0, 0.4)
             writer.grab_frame()
 
 
 def plot_video_xy(
     plot_params_rod1: dict,
     plot_params_rod2: dict,
-    plot_params_cylinder: dict = None,
-    video_name="joint_cases_video_xy.mp4",
-    fps=100,
-    cylinder: Cylinder = None,
+    video_name="video.mp4",
+    margin=0.2,
+    fps=15,
 ):  # (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"]
@@ -212,69 +133,24 @@ 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,
-    plot_params_cylinder: dict = None,
-    video_name="joint_cases_video_xz.mp4",
-    fps=100,
-    cylinder: Cylinder = None,
+    video_name="video.mp4",
+    margin=0.2,
+    fps=15,
 ):  # (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"]
@@ -295,41 +171,7 @@ 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.61])
-            plt.xlabel("x [m]")
-            plt.ylabel("z [m]")
+            plt.ylim([0, 0.41])
             writer.grab_frame()
diff --git a/examples/JointCases/spherical_joint.py b/examples/JointCases/spherical_joint.py
index 7c73b7f..b7c7a6b 100644
--- a/examples/JointCases/spherical_joint.py
+++ b/examples/JointCases/spherical_joint.py
@@ -32,15 +32,8 @@ 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(
@@ -70,16 +63,6 @@ rod2 = CosseratRod.straight_rod(
     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(
@@ -92,12 +75,6 @@ spherical_joint_sim.connect(
 ).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(
@@ -114,6 +91,7 @@ spherical_joint_sim.add_forcing_to(rod2).using(
 # 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,
@@ -127,65 +105,39 @@ spherical_joint_sim.dampen(rod2).using(
 
 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
+    MyCallBack, step_skip=1000, 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
+    MyCallBack, step_skip=1000, callback_params=pp_list_rod2
 )
 
 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 = True
+SAVE_FIGURE = False
 PLOT_VIDEO = True
 
 # plotting results
 if PLOT_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,
-    )
+    filename = "spherical_joint_test_last_node_pos_xy.png"
+    plot_position(pp_list_rod1, pp_list_rod2, filename, SAVE_FIGURE)
 
 if PLOT_VIDEO:
-    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,
-    )
+    filename = "spherical_joint_test.mp4"
+    plot_video(pp_list_rod1, pp_list_rod2, video_name=filename, margin=0.2, fps=100)
     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,
+        pp_list_rod1, pp_list_rod2, video_name=filename + "_xy.mp4", margin=0.2, fps=100
     )
     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,
+        pp_list_rod1, pp_list_rod2, video_name=filename + "_xz.mp4", margin=0.2, fps=100
     )


Test patch:
diff --git a/tests/test_joint.py b/tests/test_joint.py
index b12c179..2071225 100644
--- a/tests/test_joint.py
+++ b/tests/test_joint.py
@@ -1,11 +1,6 @@
 __doc__ = """ Joint between rods test module """
 
 # System imports
-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,
@@ -13,10 +8,8 @@ from elastica.joint import (
 )
 from numpy.testing import assert_allclose
 from elastica._rotations import _inv_rotate
-from elastica.rigidbody import Cylinder
 from elastica.rod.cosserat_rod import CosseratRod
 from elastica.utils import Tolerance
-import importlib
 import numpy as np
 import pytest
 from scipy.spatial.transform import Rotation
@@ -99,17 +92,13 @@ def test_freejoint():
         rod2.position_collection[..., rod2_index]
         - rod1.position_collection[..., rod1_index]
     )
-    end_distance = np.linalg.norm(distance)
-    if end_distance <= Tolerance.atol():
-        end_distance = 1.0
     elasticforce = k * distance
     relative_vel = (
         rod2.velocity_collection[..., rod2_index]
         - rod1.velocity_collection[..., rod1_index]
     )
-    normal_relative_vel = np.dot(relative_vel, distance) / end_distance
-    dampingforce = nu * normal_relative_vel * distance / end_distance
-    contactforce = elasticforce - dampingforce
+    dampingforce = nu * relative_vel
+    contactforce = elasticforce + dampingforce
 
     frjt = FreeJoint(k, nu)
     frjt.apply_forces(rod1, rod1_index, rod2, rod2_index)
@@ -197,17 +186,13 @@ def test_hingejoint():
         rod2.position_collection[..., rod2_index]
         - rod1.position_collection[..., rod1_index]
     )
-    end_distance = np.linalg.norm(distance)
-    if end_distance == 0:
-        end_distance = 1
     elasticforce = k * distance
     relative_vel = (
         rod2.velocity_collection[..., rod2_index]
         - rod1.velocity_collection[..., rod1_index]
     )
-    normal_relative_vel = np.dot(relative_vel, distance) / end_distance
-    dampingforce = nu * normal_relative_vel * distance / end_distance
-    contactforce = elasticforce - dampingforce
+    dampingforce = nu * relative_vel
+    contactforce = elasticforce + dampingforce
 
     hgjt = HingeJoint(k, nu, kt, normal1)
 
@@ -327,17 +312,13 @@ def test_fixedjoint(rest_euler_angle):
         rod2.position_collection[..., rod2_index]
         - rod1.position_collection[..., rod1_index]
     )
-    end_distance = np.linalg.norm(distance)
-    if end_distance == 0:
-        end_distance = 1
     elasticforce = k * distance
     relative_vel = (
         rod2.velocity_collection[..., rod2_index]
         - rod1.velocity_collection[..., rod1_index]
     )
-    normal_relative_vel = np.dot(relative_vel, distance) / end_distance
-    dampingforce = nu * normal_relative_vel * distance / end_distance
-    contactforce = elasticforce - dampingforce
+    dampingforce = nu * relative_vel
+    contactforce = elasticforce + dampingforce
 
     rest_rotation_matrix = Rotation.from_euler(
         "xyz", rest_euler_angle, degrees=False
@@ -397,9 +378,3 @@ def test_fixedjoint(rest_euler_angle):
     assert_allclose(
         rod2.external_torques[..., rod2_index], torque_rod2, atol=Tolerance.atol()
     )
-
-
-if __name__ == "__main__":
-    from pytest import main
-
-    main([__file__])