Repository: GazzolaLab/PyElastica

Issue:
Joint between Rod and RigidBody
**Describe the bug**
According to the [documentation](https://docs.cosseratrods.org/en/latest/api/connections.html#elastica.joint.FixedJoint), a numerous number of joints (and particularly the FixedJoint) should have compatibility for connecting a CosseratRod and a RigidBody (for example a Cylinder). However in practice, there are some bugs when trying to use this feature:

Error message for call `simulator.connect(rigid_body, rod, first_connect_idx=0, second_connect_idx=-1)`:
```
File "/Users/maximilianstoelzle/opt/miniconda3/envs/hsa-control/lib/python3.10/site-packages/elastica/wrappers/connections.py", line 99, in _call_connections
    connection.apply_torques(
  File "/Users/maximilianstoelzle/opt/miniconda3/envs/hsa-control/lib/python3.10/site-packages/elastica/joint.py", line 250, in apply_torques
    + rod_two.rest_lengths[index_two] * rod_one.tangents[..., index_one]
AttributeError: 'Cylinder' object has no attribute 'tangents'
```

Error message for call `simulator.connect(rod, rigid_body, first_connect_idx=0, second_connect_idx=0)`:
```
 File "/Users/maximilianstoelzle/opt/miniconda3/envs/hsa-control/lib/python3.10/site-packages/elastica/wrappers/connections.py", line 99, in _call_connections
    connection.apply_torques(
  File "/Users/maximilianstoelzle/opt/miniconda3/envs/hsa-control/lib/python3.10/site-packages/elastica/joint.py", line 250, in apply_torques
    + rod_two.rest_lengths[index_two] * rod_one.tangents[..., index_one]
AttributeError: 'Cylinder' object has no attribute 'rest_lengths'
```

Error message for call `simulator.connect(rod, rigid_body, first_connect_idx=-1, second_connect_idx=0)`:
```
 File "/Users/maximilianstoelzle/opt/miniconda3/envs/hsa-control/lib/python3.10/site-packages/elastica/wrappers/connections.py", line 99, in _call_connections
    connection.apply_torques(
  File "/Users/maximilianstoelzle/opt/miniconda3/envs/hsa-control/lib/python3.10/site-packages/elastica/joint.py", line 239, in apply_torques
    rod_two.position_collection[..., index_two + 1]
IndexError: index 1 is out of bounds for axis 1 with size 1
```

**To Reproduce**
```py
from elastica.joint import FixedJoint
from elastica.rod.cosserat_rod import CosseratRod
from elastica.rigidbody import Cylinder
rod = CosseratRod.straight_rod(
                n_elements=50,  # number of elements
                start=rod_base_position,  # Starting position of first node in rod
                direction=np.array([0.0, 1.0, 0.0]),  # Direction the rod extends
                normal=np.array([0.0, 0.0, 1.0]),  # normal vector of rod
                base_length=1.,  # original length of rod (m)
                base_radius=0.05,  # original radius of rod (m)
                density=1e3,  # density of rod (kg/m^3)
                nu=1e-3,  # Energy dissipation of rod
                youngs_modulus=youngs_modulus,
                shear_modulus=shear_modulus,
            )
rigid_body =  Cylinder(start=np.array[0., 0., 1.),
                            normal=np.array([0., 0., 1.]),
                            direction=np.array([1., 0., 0.]),
                            base_length=platform_thickness,
                            base_radius=platform_radius,
                            density=1.,  # TODO: replace with actual density of platform
                        )
simulator.connect(rigid_body, rod, first_connect_idx=0, second_connect_idx=-1).using(
                                    FixedJoint,
                                    k=1e8, kt=1e6, nu=1,
                                )
```

**Environment**
- Python=3.10.5, pip=22.1.2 
- Numpy/numba version: 22.1.2
- PyElastica version: 0.2.3
- OS, Device: macOS / macBook Pro 15'' 2019

**Expected behavior**
I expect the be able to connect a RigidBody and a CosseratRod via a (Fixed)Joint, no matter which is referenced as `rod_1` or `rod_2` in the `connect` method.

**Screenshots**
If applicable, add screenshots to help explain your problem.

**Additional context**
Add any other context about the problem here.


Discussion:
armantekinalp: @mstoelzle is the simulation in second video explodes ?
bhosale2: @mstoelzle we can focus on this one after #143 is completed, which is closer to completion.
bhosale2: @mstoelzle what's the status here? The PR says WIP do you still need to implement something?
mstoelzle: > @mstoelzle what's the status here? The PR says WIP do you still need to implement something?

@bhosale2 Although some tests are still missing, I would like to ask you to already have a look at this PR. I would like to prevent writing some tests for functionality / interfaces we might not need / do differently at the end. 
bhosale2: @mstoelzle I went through the PR briefly and noticed that 2 concepts are being implemented simultaneously here:

1. Joint between the rod and rigid body
2. The joint itself is now configurable via the `point_system` feature that you introduce here, where the entities can now be connected at different points, and not constrained to the end of the rigid body or the node of the Cosserat rod element.

In my opinion, the second feature shows some base class structure change, and also should be a separate PR in itself? Because the connection point feature also adds more configurability to joints between rods themselves. Maybe here we stick to the more frequent version of connections at ends?

Let's take a call after seeing what @skim0119 and @armantekinalp think.
mstoelzle: > @mstoelzle I went through the PR briefly and noticed that 2 concepts are being implemented simultaneously here:
> 
> 
> 
> 1. Joint between the rod and rigid body
> 
> 2. The joint itself is now configurable via the `point_system` feature that you introduce here, where the entities can now be connected at different points, and not constrained to the end of the rigid body or the node of the Cosserat rod element.
> 
> 
> 
> In my opinion, the second feature shows some base class structure change, and also should be a separate PR in itself? Because the connection point feature also adds more configurability to joints between rods themselves. Maybe here we stick to the more frequent version of connections at ends?
> 
> 
> 
> Let's take a call after seeing what @skim0119 and @armantekinalp think.

Thanks @bhosale2 for having a look at the PR. I agree with your analysis that this PR incorporates the two components you mentioned. It is important however to keep in mind the following points:

1. If we want to connect a rod with a cylinder at the nodes (e.g. center of mass of the cylinder - so the traditional way), there is actually only the tiny change in the 'HingeJoint' class necessary. The 'FreeJoint' wouldn't need any modifications.
2. However, while connecting at a node is suitable for Cosserat rods most of the time, I think it is not useful for rigid bodies in a most cases, as we would hardly want the Center of Mass of the Rigid Body to coincide with the node of the Cosserat Rod. The much larger impact can be created when we give the user the design freedom to choose at which point the joint should be attached to the rigid body: for example the bottom or top of a cylinder etc. 
3. Even if we come up with some hard-coded strategy such as that cylinders are always connected at its base at radius zero (e.g. end of cylinder), most of the code changes would still be necessary and the PR would only be slightly smaller. 
mstoelzle: > * Lets keep the previous `FreeJoint` implementation as it is and introduce a new class to connect rigid body and Cosserat Rod.

As `FixedJoint` is inheriting from `FreeJoint` and also needs to work with rigid bodies, this would require us to do a separate `FixedJoint` class as well to connect rigid bodies. 

- Does this make sense to have this much duplicate code? 
- How do you propose to call the `FreeJoint` and `FixedJoint`, which are able to connect to rigid bodies?

> * I understand why you need to connect cylinder at some other point then the COM, but what is the reason to connect rod a point different than its nodes? Since always discretization of the rod can be changed.

Thanks for your comment. I would like to divide my response into three parts:

- Allowing points as input parameters for both rigid bodies and Cosserat rod nodes makes the implementation easier, the interface cleaner and requires less _if_ statements, as we do not need to distinguish between system types / classes.
- While `point` offsets in z-dir from the Cosserat rod joint can be compensated by choosing a different discretization, as you propose, any radial offset cannot be evoked by choosing a different Cosserat rod configuration, as the mass distribution would be different.
- I currently do not have a specific example / application in mind for defining a point relative to a node. Still, I think it's nice to give the user the freedom to tailor the joint to their use-case, also with regard to connecting Cosserat rods

> * Also please remove `compute_position_of_point` method from Cosserat Rod and Rigid body and include inside the joint.py file as a free function.

Thanks - I will do that.


armantekinalp: I think moving this implementation under experiments will help us doing refactoring later on. Similarly, we can have rigid body rod connection in different examples, so future users will have more references to look. So addition to the @skim0119  comments, I would suggest renaming the rigid-body rod connection examples for spherical and fixed joint as, `rod_cylinder_spherical_joint.py` and `rod_cylinder_fixed_joint.py` and have them under different folder. 
mstoelzle: > @mstoelzle Thank you for your work! I liked your idea and implementation in general :)

Thanks everybody for all your help in reviewing and suggesting changes for all the PRs in the last few weeks. I appreciate it and I think it definitely improved the code quality!

> 
> * Lets move `point-system` implementation to the `experimental` folder.
>   
>   * Create `experimental/connection_contact_joint/rod_rigidbody_connection.py`

@skim0119 I have done that in https://github.com/GazzolaLab/PyElastica/pull/149/commits/ff77782ce9b9ffb0c0cf690ce45fdec3d472565b


> So addition to the @skim0119 comments, I would suggest renaming the rigid-body rod connection examples for spherical and fixed joint as, `rod_cylinder_spherical_joint.py` and `rod_cylinder_fixed_joint.py` and have them under different folder.

@armantekinalp I have addressed this in https://github.com/GazzolaLab/PyElastica/pull/149/commits/ff77782ce9b9ffb0c0cf690ce45fdec3d472565b as well

mstoelzle: > 



> Minor stuff, good work @mstoelzle, after this round it will be LGTM from my side.
> Also based on #163, please pull latest `update`, then modify the rod damper class name `ExponentialDamper` to `AnalyticalLinearDamper`.

Thanks for your suggestions. I addressed all of them and I also fixed a bug in the changes for the `HingeJoint`

Patch:
diff --git a/elastica/experimental/connection_contact_joint/generic_system_type_connection.py b/elastica/experimental/connection_contact_joint/generic_system_type_connection.py
new file mode 100644
index 0000000..d4a0e23
--- /dev/null
+++ b/elastica/experimental/connection_contact_joint/generic_system_type_connection.py
@@ -0,0 +1,412 @@
+__doc__ = (
+    """ Module containing joint classes to connect rods and rigid bodies together. """
+)
+__all__ = ["GenericSystemTypeFreeJoint", "GenericSystemTypeFixedJoint"]
+from elastica.joint import FreeJoint, FixedJoint
+from elastica.typing import SystemType
+from elastica.utils import Tolerance, MaxDimension
+import numpy as np
+from typing import Optional
+
+# Experimental implementation for `joint` modules: #122 #149
+#   - Enable the joint between `rod` and `rigid-body`
+#   - Allow joint to form offset from the node/COM
+#   - Generalized version for joint system
+# Blocked by:
+#   - #113
+# TODO:
+#    - [x] Tests
+#    - [ ] Physical validation / theory / literature for reference
+#    - [ ] Optimization / Numba
+#    - [ ] Benchmark
+#    - [x] Examples
+
+
+class GenericSystemTypeFreeJoint(FreeJoint):
+    """
+    Constrains the relative movement between two nodes by applying restoring forces.
+
+    Attributes
+    ----------
+    k : float
+        Stiffness coefficient of the joint.
+    nu : float
+        Damping coefficient of the joint.
+    point_system_one : numpy.ndarray
+        Describes for system one in the local coordinate system the translation from the node `index_one` (for rods)
+        or the center of mass (for rigid bodies) to the joint.
+    point_system_two : numpy.ndarray
+        Describes for system two in the local coordinate system the translation from the node `index_two` (for rods)
+        or the center of mass (for rigid bodies) to the joint.
+
+
+    Examples
+    --------
+    How to connect two Cosserat rods together using a spherical joint with a gap of 0.01 m in between.
+
+    >>> simulator.connect(rod_one, rod_two, first_connect_idx=-1, second_connect_idx=0).using(
+    ...    FreeJoint,
+    ...    k=1e4,
+    ...    nu=1,
+    ...    point_system_one=np.array([0.0, 0.0, 0.005]),
+    ...    point_system_two=np.array([0.0, 0.0, -0.005]),
+    ... )
+
+    How to connect the distal end of a CosseratRod with the base of a cylinder using a spherical joint.
+
+    >>> simulator.connect(rod, cylinder, first_connect_idx=-1, second_connect_idx=0).using(
+    ...    FreeJoint,
+    ...    k=1e4,
+    ...    nu=1,
+    ...    point_system_two=np.array([0.0, 0.0, -cylinder.length / 2.]),
+    ... )
+
+    """
+
+    # pass the k and nu for the forces
+    # also the necessary systems for the joint
+    def __init__(
+        self,
+        k: float,
+        nu: float,
+        point_system_one: Optional[np.ndarray] = None,
+        point_system_two: Optional[np.ndarray] = None,
+        **kwargs,
+    ):
+        """
+
+        Parameters
+        ----------
+        k : float
+            Stiffness coefficient of the joint.
+        nu : float
+            Damping coefficient of the joint.
+        point_system_one : Optional[numpy.ndarray]
+            Describes for system one in the local coordinate system the translation from the node `index_one` (for rods)
+            or the center of mass (for rigid bodies) to the joint.
+            (default = np.array([0.0, 0.0, 0.0]))
+        point_system_two : Optional[numpy.ndarray]
+            Describes for system two in the local coordinate system the translation from the node `index_two` (for rods)
+            or the center of mass (for rigid bodies) to the joint.
+            (default = np.array([0.0, 0.0, 0.0]))
+        """
+        super().__init__(k=k, nu=nu, **kwargs)
+
+        self.point_system_one = (
+            point_system_one
+            if point_system_one is not None
+            else np.array([0.0, 0.0, 0.0])
+        )
+        self.point_system_two = (
+            point_system_two
+            if point_system_two is not None
+            else np.array([0.0, 0.0, 0.0])
+        )
+
+    def apply_forces(
+        self,
+        system_one: SystemType,
+        index_one: int,
+        system_two: SystemType,
+        index_two: int,
+    ):
+        """
+        Apply joint force to the connected systems.
+
+        Parameters
+        ----------
+        system_one : SystemType
+            System two of the joint connection.
+        index_one : int
+            Index of first system for joint.
+        system_two : SystemType
+            System two of the joint connection.
+        index_two : int
+            Index of second system for joint.
+
+        """
+        # Compute the position in the inertial frame of the specified point.
+        # The point is defined in the local coordinate system of system one and used to attach to the joint.
+        position_system_one = compute_position_of_point(
+            system=system_one, point=self.point_system_one, index=index_one
+        )
+        # Compute the position in the inertial frame of the specified point.
+        # The point is defined in the local coordinate system of system two and used to attach to the joint.
+        position_system_two = compute_position_of_point(
+            system=system_two, point=self.point_system_two, index=index_two
+        )
+
+        # Analogue to the positions, compute the velocities of the points in the inertial frames
+        velocity_system_one = compute_velocity_of_point(
+            system=system_one, point=self.point_system_one, index=index_one
+        )
+        velocity_system_two = compute_velocity_of_point(
+            system=system_two, point=self.point_system_two, index=index_two
+        )
+
+        # Compute the translational deviation of the point belonging to system one
+        # from the point belonging to system two
+        distance_vector = position_system_two - position_system_one
+
+        # Compute elastic force using a spring formulation as a linear function of the (undesired) distance between
+        # the two systems.
+        elastic_force = self.k * distance_vector
+
+        # Compute the velocity deviation of the point belonging to system one from the point belonging to system two
+        relative_velocity = velocity_system_two - velocity_system_one
+
+        # Compute damping force considering the specified damping coefficient `nu`
+        damping_force = self.nu * relative_velocity
+
+        # compute contact force as addition of elastic force and damping force
+        contact_force = elastic_force + damping_force
+
+        # loop over the two systems
+        for i, (system, index, point, system_position) in enumerate(
+            zip(
+                [system_one, system_two],
+                [index_one, index_two],
+                [self.point_system_one, self.point_system_two],
+                [position_system_one, position_system_two],
+            )
+        ):
+            # The external force has opposite signs for the two systems:
+            # For system one: external_force = +contact_force
+            # For system two: external_force = -contact_force
+            external_force = (1 - 2 * i) * contact_force
+
+            # the contact force needs to be applied at a distance from the Center of Mass (CoM) of the rigid body
+            # or the selected node of the Cosserat rod.
+            # This generates a torque, which we also need to apply to both systems.
+            # We first compute the vector r from the node / CoM to the joint connection point.
+            distance_system_point = (
+                system_position - system.position_collection[..., index]
+            )
+            # The torque is the cross product of the distance vector and the contact force: tau = r x F
+            external_torque = np.cross(distance_system_point, external_force)
+
+            # Apply external forces and torques to both systems.
+            system.external_forces[..., index] += external_force
+            # the torque still needs to be rotated into the local coordinate system of the system
+            system.external_torques[..., index] += (
+                system.director_collection[..., index] @ external_torque
+            )
+
+    def apply_torques(
+        self,
+        system_one: SystemType,
+        index_one: int,
+        system_two: SystemType,
+        index_two: int,
+    ):
+        """
+        Apply restoring joint torques to the connected systems.
+
+        In FreeJoint class, this routine simply passes.
+
+        Parameters
+        ----------
+        system_one : SystemType
+             System two of the joint connection.
+        index_one : int
+            Index of first system for joint.
+        system_two : SystemType
+            System two of the joint connection.
+        index_two : int
+            Index of second system for joint.
+
+        """
+        pass
+
+
+class GenericSystemTypeFixedJoint(GenericSystemTypeFreeJoint):
+    """
+    The fixed joint class restricts the relative movement and rotation
+    between two nodes and elements by applying restoring forces and torques.
+
+        Attributes
+        ----------
+        k : float
+            Stiffness coefficient of the joint.
+        nu : float
+            Damping coefficient of the joint.
+        kt : float
+            Rotational stiffness coefficient of the joint.
+        nut : float
+            Rotational damping coefficient of the joint.
+        point_system_one : numpy.ndarray
+            Describes for system one in the local coordinate system the translation from the node `index_one` (for rods)
+            or the center of mass (for rigid bodies) to the joint.
+        point_system_two : numpy.ndarray
+            Describes for system two in the local coordinate system the translation from the node `index_two` (for rods)
+            or the center of mass (for rigid bodies) to the joint.
+        rest_rotation_matrix : np.ndarray
+            2D (3,3) array containing data with 'float' type.
+            Rest 3x3 rotation matrix from system one to system two at the connected elements.
+            Instead of aligning the directors of both systems directly, a desired rest rotational matrix labeled C_12*
+            is enforced.
+
+        Examples
+        --------
+        How to connect two Cosserat rods together using a fixed joint while aligning the tangents (e.g. local z-axis).
+
+        >>> simulator.connect(rod_one, rod_two).using(
+        ...    FixedJoint,
+        ...    k=1e4,
+        ...    nu=1,
+        ... )
+
+        How to connect a cosserat rod with the base of a cylinder using a fixed joint, where the cylinder is rotated
+        by 45 degrees around the y-axis.
+
+        >>> from scipy.spatial.transform import Rotation
+        ... simulator.connect(rod, cylinder).using(
+        ...    FixedJoint,
+        ...    k=1e5,
+        ...    nu=1e0,
+        ...    kt=1e3,
+        ...    nut=1e-3,
+        ...    point_system_two=np.array([0, 0, -cylinder.length / 2]),
+        ...    rest_rotation_matrix=Rotation.from_euler('y', np.pi / 4, degrees=False).as_matrix(),
+        ... )
+    """
+
+    def __init__(
+        self,
+        k: float,
+        nu: float,
+        kt: float,
+        nut: float = 0.0,
+        point_system_one: Optional[np.ndarray] = None,
+        point_system_two: Optional[np.ndarray] = None,
+        rest_rotation_matrix: Optional[np.ndarray] = None,
+        **kwargs,
+    ):
+        """
+
+        Parameters
+        ----------
+        k : float
+            Stiffness coefficient of the joint.
+        nu : float
+            Damping coefficient of the joint.
+        kt : float
+            Rotational stiffness coefficient of the joint.
+        nut : float
+            Rotational damping coefficient of the joint. (default=0.0)
+        point_system_one : Optional[numpy.ndarray]
+            Describes for system one in the local coordinate system the translation from the node `index_one` (for rods)
+            or the center of mass (for rigid bodies) to the joint.
+            (default = np.array([0.0, 0.0, 0.0]))
+        point_system_two : Optional[numpy.ndarray]
+            Describes for system two in the local coordinate system the translation from the node `index_two` (for rods)
+            or the center of mass (for rigid bodies) to the joint.
+            (default = np.array([0.0, 0.0, 0.0]))
+        rest_rotation_matrix : Optional[np.ndarray]
+            2D (3,3) array containing data with 'float' type.
+            Rest 3x3 rotation matrix from system one to system two at the connected elements.
+            If provided, the rest rotation matrix is enforced between the two systems throughout the simulation.
+            If not provided, `rest_rotation_matrix` is initialized to the identity matrix,
+            which means that a restoring torque will be applied to align the directors of both systems directly.
+            (default=None)
+        """
+        super().__init__(
+            k=k,
+            nu=nu,
+            point_system_one=point_system_one,
+            point_system_two=point_system_two,
+            **kwargs,
+        )
+
+        # set rotational spring and damping coefficients
+        self.kt = kt
+        self.nut = nut
+
+        # TODO: compute the rest rotation matrix directly during initialization
+        #  as soon as systems (e.g. `system_one` and `system_two`) and indices (e.g. `index_one` and `index_two`)
+        #  are available in the __init__
+        if rest_rotation_matrix is None:
+            rest_rotation_matrix = np.eye(3)
+        assert rest_rotation_matrix.shape == (3, 3), "Rest rotation matrix must be 3x3"
+        self.rest_rotation_matrix = rest_rotation_matrix
+
+    # Use the `apply_torques` method of the `FixedJoint` class to apply restoring torques to the connected systems.
+    apply_torques = FixedJoint.apply_torques
+
+
+def compute_position_of_point(system: SystemType, point: np.ndarray, index: int):
+    """
+    Computes the position in the inertial frame of a point specified in the local frame of
+    the specified node of the rod.
+
+    Parameters
+    ----------
+    system: SystemType
+        System to which the point belongs.
+    point : np.ndarray
+        1D (3,) numpy array containing 'float' data.
+        The point describes a position in the local frame relative to the inertial position of node
+        with index `index` and orientation of element with `index`.
+    index : int
+        Index of the node / element in the system.
+
+    Returns
+    -------
+    position : np.ndarray
+        1D (3,) numpy array containing 'float' data.
+        Position of the point in the inertial frame.
+
+    Examples
+    --------
+    Compute position in inertial frame for a point (0, 0, 1) relative to the last node of the rod.
+
+    >>> system.compute_position_of_point(np.array([0, 0, 1]), -1)
+    """
+    position = (
+        system.position_collection[..., index]
+        + system.director_collection[..., index].T @ point
+    )
+    return position
+
+
+def compute_velocity_of_point(system: SystemType, point: np.ndarray, index: int):
+    """
+    Computes the velocity in the inertial frame of point specified in the local frame of a node / element.
+
+    Parameters
+    ----------
+    system: SystemType
+        System to which the point belongs.
+    point : np.ndarray
+        1D (3,) numpy array containing 'float' data.
+        The point describes a position in the local frame relative to the inertial position of node
+        with index `index` and orientation of element with `index`.
+    index : int
+        Index of the node / element in the system.
+
+    Returns
+    -------
+    velocity : np.ndarray
+        1D (3,) numpy array containing 'float' data.
+        Velocity of the point in the inertial frame.
+
+    Examples
+    --------
+    Compute velocity in inertial frame for a point (0, 0, 1) relative to the last node of the rod.
+
+    >>> system.compute_velocity_of_point(np.array([0, 0, 1]), -1)
+    """
+    # point rotated into the inertial frame
+    point_inertial_frame = system.director_collection[..., index].T @ point
+
+    # rotate angular velocity to inertial frame
+    omega_inertial_frame = np.dot(
+        system.director_collection[..., index].T, system.omega_collection[..., index]
+    )
+
+    # apply the euler differentiation rule
+    velocity = system.velocity_collection[..., index] + np.cross(
+        omega_inertial_frame, point_inertial_frame
+    )
+
+    return velocity
diff --git a/elastica/joint.py b/elastica/joint.py
index 755b53b..4a11760 100644
--- a/elastica/joint.py
+++ b/elastica/joint.py
@@ -1,11 +1,12 @@
 __doc__ = """ Module containing joint classes to connect multiple rods together. """
 __all__ = ["FreeJoint", "HingeJoint", "FixedJoint", "ExternalContact", "SelfContact"]
-import numpy as np
-import numba
-from elastica.utils import Tolerance, MaxDimension
 from elastica._linalg import _batch_product_k_ik_to_ik
 from elastica._rotations import _inv_rotate
+from elastica.typing import SystemType
+from elastica.utils import Tolerance, MaxDimension
 from math import sqrt
+import numba
+import numpy as np
 
 
 class FreeJoint:
@@ -167,31 +168,27 @@ class HingeJoint(FreeJoint):
         self.kt = kt
 
     # Apply force is same as free joint
-    def apply_forces(self, rod_one, index_one, rod_two, index_two):
-        return super().apply_forces(rod_one, index_one, rod_two, index_two)
+    def apply_forces(self, system_one, index_one, system_two, index_two):
+        return super().apply_forces(system_one, index_one, system_two, index_two)
 
-    def apply_torques(self, rod_one, index_one, rod_two, index_two):
-        # current direction of the first element of link two
-        # also NOTE: - rod two is hinged at first element
-        link_direction = (
-            rod_two.position_collection[..., index_two + 1]
-            - rod_two.position_collection[..., index_two]
-        )
+    def apply_torques(self, system_one, index_one, system_two, index_two):
+        # current tangent direction of the `index_two` element of system two
+        system_two_tangent = system_two.director_collection[2, :, index_two]
 
-        # projection of the link direction onto the plane normal
+        # projection of the tangent of system two onto the plane normal
         force_direction = (
-            -np.dot(link_direction, self.normal_direction) * self.normal_direction
+            -np.dot(system_two_tangent, self.normal_direction) * self.normal_direction
         )
 
         # compute the restoring torque
-        torque = self.kt * np.cross(link_direction, force_direction)
+        torque = self.kt * np.cross(system_two_tangent, force_direction)
 
         # The opposite torque will be applied on link one
-        rod_one.external_torques[..., index_one] -= (
-            rod_one.director_collection[..., index_one] @ torque
+        system_one.external_torques[..., index_one] -= (
+            system_one.director_collection[..., index_one] @ torque
         )
-        rod_two.external_torques[..., index_two] += (
-            rod_two.director_collection[..., index_two] @ torque
+        system_two.external_torques[..., index_two] += (
+            system_two.director_collection[..., index_two] @ torque
         )
 
 
@@ -893,7 +890,7 @@ class ExternalContact(FreeJoint):
     --------
     How to define contact between rod and cylinder.
 
-    >>> simulator.connect(rod, cylidner).using(
+    >>> simulator.connect(rod, cylinder).using(
     ...    ExternalContact,
     ...    k=1e4,
     ...    nu=10,
diff --git a/examples/JointCases/experimental/generic_system_type_fixed_joint.py b/examples/JointCases/experimental/generic_system_type_fixed_joint.py
new file mode 100644
index 0000000..551e365
--- /dev/null
+++ b/examples/JointCases/experimental/generic_system_type_fixed_joint.py
@@ -0,0 +1,201 @@
+__doc__ = """Fixed joint example, for detailed explanation refer to Zhang et. al. Nature Comm.  methods section."""
+
+import numpy as np
+from elastica import *
+from elastica.experimental.connection_contact_joint.generic_system_type_connection import (
+    GenericSystemTypeFixedJoint,
+)
+from examples.JointCases.joint_cases_postprocessing import (
+    plot_position,
+    plot_video,
+    plot_video_xy,
+    plot_video_xz,
+)
+
+
+class FixedJointSimulator(
+    BaseSystemCollection, Constraints, Connections, Forcing, Damping, CallBacks
+):
+    pass
+
+
+fixed_joint_sim = FixedJointSimulator()
+
+# setting up test params
+n_elem = 10
+direction = np.array([0.0, 0.0, 1.0])
+normal = np.array([0.0, 1.0, 0.0])
+roll_direction = np.cross(direction, normal)
+base_length = 0.2
+base_radius = 0.007
+base_area = np.pi * base_radius ** 2
+density = 1750
+E = 3e7
+poisson_ratio = 0.5
+shear_modulus = E / (poisson_ratio + 1.0)
+
+# setting up time params
+final_time = 10
+dt = 5e-5
+fps = 100  # fps of the video
+step_skip = int(1 / (dt * fps))
+
+start_rod_1 = np.zeros((3,))
+start_rod_2 = start_rod_1 + direction * base_length
+start_cylinder = start_rod_2 + direction * base_length
+
+# Create rod 1
+rod1 = CosseratRod.straight_rod(
+    n_elem,
+    start_rod_1,
+    direction,
+    normal,
+    base_length,
+    base_radius,
+    density,
+    0.0,  # internal damping constant, deprecated in v0.3.0
+    E,
+    shear_modulus=shear_modulus,
+)
+fixed_joint_sim.append(rod1)
+# Create rod 2
+rod2 = CosseratRod.straight_rod(
+    n_elem,
+    start_rod_2,
+    direction,
+    normal,
+    base_length,
+    base_radius,
+    density,
+    0.0,  # internal damping constant, deprecated in v0.3.0
+    E,
+    shear_modulus=shear_modulus,
+)
+fixed_joint_sim.append(rod2)
+cylinder = Cylinder(
+    start=start_cylinder,
+    direction=direction,
+    normal=normal,
+    base_length=base_length,
+    base_radius=base_radius,
+    density=density,
+)
+fixed_joint_sim.append(cylinder)
+
+# Apply boundary conditions to rod1.
+fixed_joint_sim.constrain(rod1).using(
+    OneEndFixedBC, constrained_position_idx=(0,), constrained_director_idx=(0,)
+)
+
+# Connect rod 1 and rod 2
+fixed_joint_sim.connect(
+    first_rod=rod1, second_rod=rod2, first_connect_idx=-1, second_connect_idx=0
+).using(
+    GenericSystemTypeFixedJoint,
+    k=1e5,
+    nu=0e0,
+    kt=1e1,
+    nut=0e0,
+)
+# Connect rod 2 and cylinder
+fixed_joint_sim.connect(
+    first_rod=rod2, second_rod=cylinder, first_connect_idx=-1, second_connect_idx=0
+).using(
+    GenericSystemTypeFixedJoint,
+    k=1e5,
+    nu=0e0,
+    kt=1e1,
+    nut=0e0,
+    point_system_two=np.array([0.0, 0.0, -cylinder.length / 2]),
+)
+
+# Add forces to rod2
+fixed_joint_sim.add_forcing_to(rod2).using(
+    EndpointForcesSinusoidal,
+    start_force_mag=0,
+    end_force_mag=5e-3,
+    ramp_up_time=0.2,
+    tangent_direction=direction,
+    normal_direction=normal,
+)
+
+# add damping
+# old damping model (deprecated in v0.3.0) values
+# damping_constant = 0.4
+# dt = 1e-5
+damping_constant = 0.4
+fixed_joint_sim.dampen(rod1).using(
+    AnalyticalLinearDamper,
+    damping_constant=damping_constant,
+    time_step=dt,
+)
+fixed_joint_sim.dampen(rod2).using(
+    AnalyticalLinearDamper,
+    damping_constant=damping_constant,
+    time_step=dt,
+)
+
+pp_list_rod1 = defaultdict(list)
+pp_list_rod2 = defaultdict(list)
+pp_list_cylinder = defaultdict(list)
+
+fixed_joint_sim.collect_diagnostics(rod1).using(
+    MyCallBack, step_skip=step_skip, callback_params=pp_list_rod1
+)
+fixed_joint_sim.collect_diagnostics(rod2).using(
+    MyCallBack, step_skip=step_skip, callback_params=pp_list_rod2
+)
+fixed_joint_sim.collect_diagnostics(cylinder).using(
+    MyCallBack, step_skip=step_skip, callback_params=pp_list_cylinder
+)
+
+fixed_joint_sim.finalize()
+timestepper = PositionVerlet()
+# timestepper = PEFRL()
+
+dl = base_length / n_elem
+total_steps = int(final_time / dt)
+print("Total steps", total_steps)
+integrate(timestepper, fixed_joint_sim, final_time, total_steps)
+
+PLOT_FIGURE = True
+SAVE_FIGURE = True
+PLOT_VIDEO = True
+
+# plotting results
+if PLOT_FIGURE:
+    filename = "generic_system_type_fixed_joint_example_last_node_pos_xy.png"
+    plot_position(
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=pp_list_cylinder,
+        filename=filename,
+        SAVE_FIGURE=SAVE_FIGURE,
+    )
+
+if PLOT_VIDEO:
+    filename = "generic_system_type_fixed_joint_example"
+    plot_video(
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=pp_list_cylinder,
+        cylinder=cylinder,
+        video_name=filename + ".mp4",
+        fps=fps,
+    )
+    plot_video_xy(
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=pp_list_cylinder,
+        cylinder=cylinder,
+        video_name=filename + "_xy.mp4",
+        fps=fps,
+    )
+    plot_video_xz(
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=pp_list_cylinder,
+        cylinder=cylinder,
+        video_name=filename + "_xz.mp4",
+        fps=fps,
+    )
diff --git a/examples/JointCases/experimental/generic_system_type_spherical_joint.py b/examples/JointCases/experimental/generic_system_type_spherical_joint.py
new file mode 100644
index 0000000..7d447b5
--- /dev/null
+++ b/examples/JointCases/experimental/generic_system_type_spherical_joint.py
@@ -0,0 +1,197 @@
+__doc__ = """Spherical(Free) joint example, for detailed explanation refer to Zhang et. al. Nature Comm.  
+methods section."""
+
+import numpy as np
+from elastica import *
+from elastica.experimental.connection_contact_joint.generic_system_type_connection import (
+    GenericSystemTypeFreeJoint,
+)
+from examples.JointCases.joint_cases_postprocessing import (
+    plot_position,
+    plot_video,
+    plot_video_xy,
+    plot_video_xz,
+)
+
+
+class SphericalJointSimulator(
+    BaseSystemCollection, Constraints, Connections, Forcing, Damping, CallBacks
+):
+    pass
+
+
+spherical_joint_sim = SphericalJointSimulator()
+
+# setting up test params
+n_elem = 10
+direction = np.array([0.0, 0.0, 1.0])
+normal = np.array([0.0, 1.0, 0.0])
+roll_direction = np.cross(direction, normal)
+base_length = 0.2
+base_radius = 0.007
+base_area = np.pi * base_radius ** 2
+density = 1750
+E = 3e7
+poisson_ratio = 0.5
+shear_modulus = E / (poisson_ratio + 1.0)
+
+# setting up time params
+final_time = 10
+dt = 5e-5
+fps = 100  # fps of the video
+step_skip = int(1 / (dt * fps))
+
+start_rod_1 = np.zeros((3,))
+start_rod_2 = start_rod_1 + direction * base_length
+start_cylinder = start_rod_2 + direction * base_length
+
+# Create rod 1
+rod1 = CosseratRod.straight_rod(
+    n_elem,
+    start_rod_1,
+    direction,
+    normal,
+    base_length,
+    base_radius,
+    density,
+    0.0,  # internal damping constant, deprecated in v0.3.0
+    E,
+    shear_modulus=shear_modulus,
+)
+spherical_joint_sim.append(rod1)
+# Create rod 2
+rod2 = CosseratRod.straight_rod(
+    n_elem,
+    start_rod_2,
+    direction,
+    normal,
+    base_length,
+    base_radius,
+    density,
+    0.0,  # internal damping constant, deprecated in v0.3.0
+    E,
+    shear_modulus=shear_modulus,
+)
+spherical_joint_sim.append(rod2)
+# Create cylinder
+cylinder = Cylinder(
+    start=start_cylinder,
+    direction=direction,
+    normal=normal,
+    base_length=base_length,
+    base_radius=base_radius,
+    density=density,
+)
+spherical_joint_sim.append(cylinder)
+
+# Apply boundary conditions to rod1.
+spherical_joint_sim.constrain(rod1).using(
+    OneEndFixedBC, constrained_position_idx=(0,), constrained_director_idx=(0,)
+)
+
+# Connect rod 1 and rod 2
+spherical_joint_sim.connect(
+    first_rod=rod1, second_rod=rod2, first_connect_idx=-1, second_connect_idx=0
+).using(
+    GenericSystemTypeFreeJoint, k=1e5, nu=0
+)  # k=kg/s2 nu=kg/s 1e-2
+# Connect rod 2 and cylinder
+spherical_joint_sim.connect(
+    first_rod=rod2, second_rod=cylinder, first_connect_idx=-1, second_connect_idx=0
+).using(
+    GenericSystemTypeFreeJoint,
+    k=1e5,
+    nu=0,
+    point_system_two=np.array([0.0, 0.0, -cylinder.length / 2]),
+)
+
+# Add forces to rod2
+spherical_joint_sim.add_forcing_to(rod2).using(
+    EndpointForcesSinusoidal,
+    start_force_mag=0,
+    end_force_mag=5e-3,
+    ramp_up_time=0.2,
+    tangent_direction=direction,
+    normal_direction=normal,
+)
+
+# add damping
+# old damping model (deprecated in v0.3.0) values
+# damping_constant = 4e-3
+# dt = 1e-5
+damping_constant = 4e-3
+spherical_joint_sim.dampen(rod1).using(
+    AnalyticalLinearDamper,
+    damping_constant=damping_constant,
+    time_step=dt,
+)
+spherical_joint_sim.dampen(rod2).using(
+    AnalyticalLinearDamper,
+    damping_constant=damping_constant,
+    time_step=dt,
+)
+
+pp_list_rod1 = defaultdict(list)
+pp_list_rod2 = defaultdict(list)
+pp_list_cylinder = defaultdict(list)
+
+spherical_joint_sim.collect_diagnostics(rod1).using(
+    MyCallBack, step_skip=step_skip, callback_params=pp_list_rod1
+)
+spherical_joint_sim.collect_diagnostics(rod2).using(
+    MyCallBack, step_skip=step_skip, callback_params=pp_list_rod2
+)
+spherical_joint_sim.collect_diagnostics(cylinder).using(
+    MyCallBack, step_skip=step_skip, callback_params=pp_list_cylinder
+)
+
+spherical_joint_sim.finalize()
+timestepper = PositionVerlet()
+# timestepper = PEFRL()
+
+dl = base_length / n_elem
+total_steps = int(final_time / dt)
+print("Total steps", total_steps)
+integrate(timestepper, spherical_joint_sim, final_time, total_steps)
+
+PLOT_FIGURE = True
+SAVE_FIGURE = True
+PLOT_VIDEO = True
+
+# plotting results
+if PLOT_FIGURE:
+    filename = "generic_system_type_spherical_joint_example_last_node_pos_xy.png"
+    plot_position(
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=pp_list_cylinder,
+        filename=filename,
+        SAVE_FIGURE=SAVE_FIGURE,
+    )
+
+if PLOT_VIDEO:
+    filename = "generic_system_type_spherical_joint_example"
+    plot_video(
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=pp_list_cylinder,
+        cylinder=cylinder,
+        video_name=filename + ".mp4",
+        fps=fps,
+    )
+    plot_video_xy(
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=pp_list_cylinder,
+        cylinder=cylinder,
+        video_name=filename + "_xy.mp4",
+        fps=fps,
+    )
+    plot_video_xz(
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=pp_list_cylinder,
+        cylinder=cylinder,
+        video_name=filename + "_xz.mp4",
+        fps=fps,
+    )
diff --git a/examples/JointCases/fixed_joint.py b/examples/JointCases/fixed_joint.py
index 0f6b63e..46a7e66 100644
--- a/examples/JointCases/fixed_joint.py
+++ b/examples/JointCases/fixed_joint.py
@@ -31,6 +31,12 @@ 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
 
@@ -71,7 +77,13 @@ 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=0, kt=5e3)
+).using(
+    FixedJoint,
+    k=1e5,
+    nu=0e0,
+    kt=1e1,
+    nut=0e0,
+)
 
 # Add forces to rod2
 fixed_joint_sim.add_forcing_to(rod2).using(
@@ -88,7 +100,6 @@ 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,
@@ -104,37 +115,56 @@ pp_list_rod1 = defaultdict(list)
 pp_list_rod2 = defaultdict(list)
 
 fixed_joint_sim.collect_diagnostics(rod1).using(
-    MyCallBack, step_skip=1000, callback_params=pp_list_rod1
+    MyCallBack, step_skip=step_skip, callback_params=pp_list_rod1
 )
 fixed_joint_sim.collect_diagnostics(rod2).using(
-    MyCallBack, step_skip=1000, callback_params=pp_list_rod2
+    MyCallBack, step_skip=step_skip, callback_params=pp_list_rod2
 )
 
 fixed_joint_sim.finalize()
 timestepper = PositionVerlet()
 # timestepper = PEFRL()
 
-final_time = 10
 dl = base_length / n_elem
 total_steps = int(final_time / dt)
 print("Total steps", total_steps)
 integrate(timestepper, fixed_joint_sim, final_time, total_steps)
 
 PLOT_FIGURE = True
-SAVE_FIGURE = False
+SAVE_FIGURE = True
 PLOT_VIDEO = True
 
 # plotting results
 if PLOT_FIGURE:
-    filename = "fixed_joint_test.png"
-    plot_position(pp_list_rod1, pp_list_rod2, filename, SAVE_FIGURE)
+    filename = "fixed_joint_example_last_node_pos_xy.png"
+    plot_position(
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=None,
+        filename=filename,
+        SAVE_FIGURE=SAVE_FIGURE,
+    )
 
 if PLOT_VIDEO:
-    filename = "fixed_joint_test.mp4"
-    plot_video(pp_list_rod1, pp_list_rod2, video_name=filename, margin=0.2, fps=100)
+    filename = "fixed_joint_example"
+    plot_video(
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=None,
+        video_name=filename + ".mp4",
+        fps=fps,
+    )
     plot_video_xy(
-        pp_list_rod1, pp_list_rod2, video_name=filename + "_xy.mp4", margin=0.2, fps=100
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=None,
+        video_name=filename + "_xy.mp4",
+        fps=fps,
     )
     plot_video_xz(
-        pp_list_rod1, pp_list_rod2, video_name=filename + "_xz.mp4", margin=0.2, fps=100
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=None,
+        video_name=filename + "_xz.mp4",
+        fps=fps,
     )
diff --git a/examples/JointCases/fixed_joint_torsion.py b/examples/JointCases/fixed_joint_torsion.py
index 27a281a..bd08f9b 100644
--- a/examples/JointCases/fixed_joint_torsion.py
+++ b/examples/JointCases/fixed_joint_torsion.py
@@ -142,30 +142,36 @@ 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)
+    filename = "fixed_joint_torsion_example_last_node_pos_xy.png"
+    plot_position(
+        pp_list_rod1,
+        pp_list_rod2,
+        plot_params_cylinder=None,
+        filename=filename,
+        SAVE_FIGURE=SAVE_FIGURE,
+    )
 
 if PLOT_VIDEO:
-    filename = "fixed_joint_torsion_test"
+    filename = "fixed_joint_torsion_example"
     fps = 100  # Hz
     plot_video(
         pp_list_rod1,
         pp_list_rod2,
+        plot_params_cylinder=None,
         video_name=filename + ".mp4",
-        margin=0.2,
         fps=fps,
     )
     plot_video_xy(
         pp_list_rod1,
         pp_list_rod2,
+        plot_params_cylinder=None,
         video_name=filename + "_xy.mp4",
-        margin=0.2,
         fps=fps,
     )
     plot_video_xz(
         pp_list_rod1,
         pp_list_rod2,
+        plot_params_cylinder=None,
         video_name=filename + "_xz.mp4",
-        margin=0.2,
         fps=fps,
     )
diff --git a/examples/JointCases/hinge_joint.py b/examples/JointCases/hinge_joint.py
index 7f9a9b9..226394e 100644
--- a/examples/JointCases/hinge_joint.py
+++ b/examples/JointCases/hinge_joint.py
@@ -72,7 +72,7 @@ hinge_joint_sim.constrain(rod1).using(
 hinge_joint_sim.connect(
     first_rod=rod1, second_rod=rod2, first_connect_idx=-1, second_connect_idx=0
 ).using(
-    HingeJoint, k=1e5, nu=0, kt=5e3, normal_direction=roll_direction
+    HingeJoint, k=1e5, nu=0, kt=1e1, normal_direction=roll_direction
 )  # 1e-2
 
 # Add forces to rod2
@@ -128,15 +128,35 @@ PLOT_VIDEO = True
 
 # plotting results
 if PLOT_FIGURE:
-    filename = "hinge_joint_test.png"
-    plot_position(pp_list_rod1, pp_list_rod2, filename, SAVE_FIGURE)
+    filename = "hinge_joint_example_last_node_pos_xy.png"
+    plot_position(
+        pp_list_rod1,
+        pp_list_rod2,
+        plot_params_cylinder=None,
+        filename=filename,
+        SAVE_FIGURE=SAVE_FIGURE,
+    )
 
 if PLOT_VIDEO:
-    filename = "hinge_joint_test.mp4"
-    plot_video(pp_list_rod1, pp_list_rod2, video_name=filename, margin=0.2, fps=100)
+    filename = "hinge_joint_example"
+    plot_video(
+        pp_list_rod1,
+        pp_list_rod2,
+        plot_params_cylinder=None,
+        video_name=filename + ".mp4",
+        fps=100,
+    )
     plot_video_xy(
-        pp_list_rod1, pp_list_rod2, video_name=filename + "_xy.mp4", margin=0.2, fps=100
+        pp_list_rod1,
+        pp_list_rod2,
+        plot_params_cylinder=None,
+        video_name=filename + "_xy.mp4",
+        fps=100,
     )
     plot_video_xz(
-        pp_list_rod1, pp_list_rod2, video_name=filename + "_xz.mp4", margin=0.2, fps=100
+        pp_list_rod1,
+        pp_list_rod2,
+        plot_params_cylinder=None,
+        video_name=filename + "_xz.mp4",
+        fps=100,
     )
diff --git a/examples/JointCases/joint_cases_postprocessing.py b/examples/JointCases/joint_cases_postprocessing.py
index d7ea143..c1ca4dd 100644
--- a/examples/JointCases/joint_cases_postprocessing.py
+++ b/examples/JointCases/joint_cases_postprocessing.py
@@ -4,16 +4,24 @@ 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,
-    filename="spherical_joint_test.png",
+    plot_params_cylinder: dict = None,
+    filename="joint_cases_last_node_pos_xy.png",
     SAVE_FIGURE=False,
 ):
-
     position_of_rod1 = np.array(plot_params_rod1["position"])
     position_of_rod2 = np.array(plot_params_rod2["position"])
+    position_of_cylinder = (
+        np.array(plot_params_cylinder["position"])
+        if plot_params_cylinder is not None
+        else None
+    )
 
     fig = plt.figure(figsize=(10, 10), frameon=True, dpi=150)
     ax = fig.add_subplot(111)
@@ -27,8 +35,17 @@ def plot_position(
         c=to_rgb("xkcd:bluish"),
         label="rod2",
     )
+    if position_of_cylinder is not None:
+        ax.plot(
+            position_of_cylinder[:, 0, -1],
+            position_of_cylinder[:, 1, -1],
+            c=to_rgb("xkcd:greenish"),
+            label="cylinder",
+        )
 
     fig.legend(prop={"size": 20})
+    plt.xlabel("x")
+    plt.ylabel("y")
 
     plt.show()
 
@@ -37,6 +54,9 @@ def plot_position(
 
 
 def plot_orientation(title, time, directors):
+    """
+    Plot the orientation of one node
+    """
     quat = []
     for t in range(len(time)):
         quat_t = Rotation.from_matrix(directors[t].T).as_quat()
@@ -58,15 +78,26 @@ def plot_orientation(title, time, directors):
 def plot_video(
     plot_params_rod1: dict,
     plot_params_rod2: dict,
-    video_name="video.mp4",
-    margin=0.2,
-    fps=15,
+    plot_params_cylinder: dict = None,
+    video_name="joint_cases_video.mp4",
+    fps=100,
+    cylinder: Cylinder = None,
 ):  # (time step, x/y/z, node)
     import matplotlib.animation as manimation
 
     time = plot_params_rod1["time"]
     position_of_rod1 = np.array(plot_params_rod1["position"])
     position_of_rod2 = np.array(plot_params_rod2["position"])
+    position_of_cylinder = (
+        np.array(plot_params_cylinder["position"])
+        if plot_params_cylinder is not None
+        else None
+    )
+    director_of_cylinder = (
+        np.array(plot_params_cylinder["directors"])
+        if plot_params_cylinder is not None
+        else None
+    )
 
     print("plot video")
     FFMpegWriter = manimation.writers["ffmpeg"]
@@ -94,25 +125,73 @@ def plot_video(
                 c=to_rgb("xkcd:bluish"),
                 label="rod2",
             )
+            if position_of_cylinder is not None:
+                ax.plot(
+                    position_of_cylinder[time, 0],
+                    position_of_cylinder[time, 1],
+                    position_of_cylinder[time, 2],
+                    "o",
+                    c=to_rgb("xkcd:greenish"),
+                    label="Cylinder CoM",
+                )
+                if cylinder is not None:
+                    cylinder_axis_points = np.zeros((3, 10))
+                    cylinder_axis_points[2, :] = np.linspace(
+                        start=-cylinder.length.squeeze() / 2,
+                        stop=cylinder.length.squeeze() / 2,
+                        num=cylinder_axis_points.shape[1],
+                    )
+                    cylinder_director = director_of_cylinder[time, ...]
+                    cylinder_director_batched = np.repeat(
+                        cylinder_director, repeats=cylinder_axis_points.shape[1], axis=2
+                    )
+                    # rotate points into inertial frame
+                    cylinder_axis_points = _batch_matvec(
+                        cylinder_director_batched.transpose((1, 0, 2)),
+                        cylinder_axis_points,
+                    )
+                    # add offset position of CoM
+                    cylinder_axis_points += position_of_cylinder[time, ...]
+                    ax.plot(
+                        cylinder_axis_points[0, :],
+                        cylinder_axis_points[1, :],
+                        cylinder_axis_points[2, :],
+                        c=to_rgb("xkcd:greenish"),
+                        label="Cylinder axis",
+                    )
 
             ax.set_xlim(-0.25, 0.25)
             ax.set_ylim(-0.25, 0.25)
-            ax.set_zlim(0, 0.4)
+            ax.set_zlim(0, 0.61)
+            ax.set_xlabel("x [m]")
+            ax.set_ylabel("y [m]")
+            ax.set_zlabel("z [m]")
             writer.grab_frame()
 
 
 def plot_video_xy(
     plot_params_rod1: dict,
     plot_params_rod2: dict,
-    video_name="video.mp4",
-    margin=0.2,
-    fps=15,
+    plot_params_cylinder: dict = None,
+    video_name="joint_cases_video_xy.mp4",
+    fps=100,
+    cylinder: Cylinder = None,
 ):  # (time step, x/y/z, node)
     import matplotlib.animation as manimation
 
     time = plot_params_rod1["time"]
     position_of_rod1 = np.array(plot_params_rod1["position"])
     position_of_rod2 = np.array(plot_params_rod2["position"])
+    position_of_cylinder = (
+        np.array(plot_params_cylinder["position"])
+        if plot_params_cylinder is not None
+        else None
+    )
+    director_of_cylinder = (
+        np.array(plot_params_cylinder["directors"])
+        if plot_params_cylinder is not None
+        else None
+    )
 
     print("plot video xy")
     FFMpegWriter = manimation.writers["ffmpeg"]
@@ -133,24 +212,69 @@ def plot_video_xy(
                 c=to_rgb("xkcd:bluish"),
                 label="rod2",
             )
+            if position_of_cylinder is not None:
+                plt.plot(
+                    position_of_cylinder[time, 0],
+                    position_of_cylinder[time, 1],
+                    "o",
+                    c=to_rgb("xkcd:greenish"),
+                    label="cylinder",
+                )
+                if cylinder is not None:
+                    cylinder_axis_points = np.zeros((3, 10))
+                    cylinder_axis_points[2, :] = np.linspace(
+                        start=-cylinder.length.squeeze() / 2,
+                        stop=cylinder.length.squeeze() / 2,
+                        num=cylinder_axis_points.shape[1],
+                    )
+                    cylinder_director = director_of_cylinder[time, ...]
+                    cylinder_director_batched = np.repeat(
+                        cylinder_director, repeats=cylinder_axis_points.shape[1], axis=2
+                    )
+                    # rotate points into inertial frame
+                    cylinder_axis_points = _batch_matvec(
+                        cylinder_director_batched.transpose((1, 0, 2)),
+                        cylinder_axis_points,
+                    )
+                    # add offset position of CoM
+                    cylinder_axis_points += position_of_cylinder[time, ...]
+                    plt.plot(
+                        cylinder_axis_points[0, :],
+                        cylinder_axis_points[1, :],
+                        c=to_rgb("xkcd:greenish"),
+                        label="Cylinder axis",
+                    )
 
             plt.xlim([-0.25, 0.25])
             plt.ylim([-0.25, 0.25])
+            plt.xlabel("x [m]")
+            plt.ylabel("y [m]")
             writer.grab_frame()
 
 
 def plot_video_xz(
     plot_params_rod1: dict,
     plot_params_rod2: dict,
-    video_name="video.mp4",
-    margin=0.2,
-    fps=15,
+    plot_params_cylinder: dict = None,
+    video_name="joint_cases_video_xz.mp4",
+    fps=100,
+    cylinder: Cylinder = None,
 ):  # (time step, x/y/z, node)
     import matplotlib.animation as manimation
 
     time = plot_params_rod1["time"]
     position_of_rod1 = np.array(plot_params_rod1["position"])
     position_of_rod2 = np.array(plot_params_rod2["position"])
+    position_of_cylinder = (
+        np.array(plot_params_cylinder["position"])
+        if plot_params_cylinder is not None
+        else None
+    )
+    director_of_cylinder = (
+        np.array(plot_params_cylinder["directors"])
+        if plot_params_cylinder is not None
+        else None
+    )
 
     print("plot video xz")
     FFMpegWriter = manimation.writers["ffmpeg"]
@@ -171,7 +295,41 @@ def plot_video_xz(
                 c=to_rgb("xkcd:bluish"),
                 label="rod2",
             )
+            if position_of_cylinder is not None:
+                plt.plot(
+                    position_of_cylinder[time, 0],
+                    position_of_cylinder[time, 2],
+                    "o",
+                    c=to_rgb("xkcd:greenish"),
+                    label="cylinder",
+                )
+                if cylinder is not None:
+                    cylinder_axis_points = np.zeros((3, 10))
+                    cylinder_axis_points[2, :] = np.linspace(
+                        start=-cylinder.length.squeeze() / 2,
+                        stop=cylinder.length.squeeze() / 2,
+                        num=cylinder_axis_points.shape[1],
+                    )
+                    cylinder_director = director_of_cylinder[time, ...]
+                    cylinder_director_batched = np.repeat(
+                        cylinder_director, repeats=cylinder_axis_points.shape[1], axis=2
+                    )
+                    # rotate points into inertial frame
+                    cylinder_axis_points = _batch_matvec(
+                        cylinder_director_batched.transpose((1, 0, 2)),
+                        cylinder_axis_points,
+                    )
+                    # add offset position of CoM
+                    cylinder_axis_points += position_of_cylinder[time, ...]
+                    plt.plot(
+                        cylinder_axis_points[0, :],
+                        cylinder_axis_points[2, :],
+                        c=to_rgb("xkcd:greenish"),
+                        label="Cylinder axis",
+                    )
 
             plt.xlim([-0.25, 0.25])
-            plt.ylim([0, 0.41])
+            plt.ylim([0, 0.61])
+            plt.xlabel("x [m]")
+            plt.ylabel("z [m]")
             writer.grab_frame()
diff --git a/examples/JointCases/spherical_joint.py b/examples/JointCases/spherical_joint.py
index b7c7a6b..7c73b7f 100644
--- a/examples/JointCases/spherical_joint.py
+++ b/examples/JointCases/spherical_joint.py
@@ -32,8 +32,15 @@ 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(
@@ -63,6 +70,16 @@ 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(
@@ -75,6 +92,12 @@ 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(
@@ -91,7 +114,6 @@ 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,
@@ -105,39 +127,65 @@ 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=1000, callback_params=pp_list_rod1
+    MyCallBack, step_skip=step_skip, callback_params=pp_list_rod1
 )
 spherical_joint_sim.collect_diagnostics(rod2).using(
-    MyCallBack, step_skip=1000, callback_params=pp_list_rod2
+    MyCallBack, step_skip=step_skip, callback_params=pp_list_rod2
+)
+spherical_joint_sim.collect_diagnostics(cylinder).using(
+    MyCallBack, step_skip=step_skip, callback_params=pp_list_cylinder
 )
 
 spherical_joint_sim.finalize()
 timestepper = PositionVerlet()
 # timestepper = PEFRL()
 
-final_time = 10
 dl = base_length / n_elem
 total_steps = int(final_time / dt)
 print("Total steps", total_steps)
 integrate(timestepper, spherical_joint_sim, final_time, total_steps)
 
 PLOT_FIGURE = True
-SAVE_FIGURE = False
+SAVE_FIGURE = True
 PLOT_VIDEO = True
 
 # plotting results
 if PLOT_FIGURE:
-    filename = "spherical_joint_test_last_node_pos_xy.png"
-    plot_position(pp_list_rod1, pp_list_rod2, filename, SAVE_FIGURE)
+    filename = "spherical_joint_example_last_node_pos_xy.png"
+    plot_position(
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=pp_list_cylinder,
+        filename=filename,
+        SAVE_FIGURE=SAVE_FIGURE,
+    )
 
 if PLOT_VIDEO:
-    filename = "spherical_joint_test.mp4"
-    plot_video(pp_list_rod1, pp_list_rod2, video_name=filename, margin=0.2, fps=100)
+    filename = "spherical_joint_example"
+    plot_video(
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=pp_list_cylinder,
+        cylinder=cylinder,
+        video_name=filename + ".mp4",
+        fps=fps,
+    )
     plot_video_xy(
-        pp_list_rod1, pp_list_rod2, video_name=filename + "_xy.mp4", margin=0.2, fps=100
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=pp_list_cylinder,
+        cylinder=cylinder,
+        video_name=filename + "_xy.mp4",
+        fps=fps,
     )
     plot_video_xz(
-        pp_list_rod1, pp_list_rod2, video_name=filename + "_xz.mp4", margin=0.2, fps=100
+        plot_params_rod1=pp_list_rod1,
+        plot_params_rod2=pp_list_rod2,
+        plot_params_cylinder=pp_list_cylinder,
+        cylinder=cylinder,
+        video_name=filename + "_xz.mp4",
+        fps=fps,
     )


Test patch:
diff --git a/tests/test_experimental/test_generic_system_type_joint.py b/tests/test_experimental/test_generic_system_type_joint.py
new file mode 100644
index 0000000..79a8797
--- /dev/null
+++ b/tests/test_experimental/test_generic_system_type_joint.py
@@ -0,0 +1,277 @@
+__doc__ = """ Joints for generic system types test module """
+
+# System imports
+from elastica.experimental.connection_contact_joint.generic_system_type_connection import (
+    GenericSystemTypeFreeJoint,
+    compute_position_of_point,
+    compute_velocity_of_point,
+)
+from numpy.testing import assert_allclose
+from elastica.rigidbody import Cylinder, Sphere, RigidBodyBase
+from elastica.rod.cosserat_rod import CosseratRod
+from elastica.utils import Tolerance
+import numpy as np
+import pytest
+from typing import *
+
+# seed random number generator
+rng = np.random.default_rng(0)
+
+
+def init_system(system_class, origin: np.array = np.array([0.0, 0.0, 0.0])):
+    # Some rod properties. We need them for constructor, they are not used.
+    n = 4  # Number of elements
+    normal = np.array([1.0, 0.0, 0.0])
+    direction = np.array([0.0, 0.0, 1.0])
+    base_length = 1
+    base_radius = 0.2
+    density = 1
+    nu = 0.1
+
+    # Youngs Modulus [Pa]
+    youngs_modulus = 1e6
+    # poisson ratio
+    poisson_ratio = 0.5
+    shear_modulus = youngs_modulus / (poisson_ratio + 1.0)
+
+    if system_class == CosseratRod:
+        system = CosseratRod.straight_rod(
+            n,
+            origin,
+            direction,
+            normal,
+            base_length,
+            base_radius,
+            density,
+            nu,
+            youngs_modulus,
+            shear_modulus=shear_modulus,
+        )
+    elif system_class == Cylinder:
+        system = Cylinder(
+            start=(origin - base_length / 2 * direction),
+            direction=direction,
+            normal=normal,
+            base_length=base_length,
+            base_radius=base_radius,
+            density=density,
+        )
+    elif system_class == Sphere:
+        system = Sphere(origin, base_radius, density)
+    else:
+        raise ValueError
+
+    return system
+
+
+@pytest.mark.parametrize("system_one_class", [CosseratRod, Sphere, Cylinder])
+@pytest.mark.parametrize("system_two_class", [CosseratRod, Sphere, Cylinder])
+@pytest.mark.parametrize(
+    "point_system_one",
+    [
+        np.zeros((3,)),
+        np.array([0.0, 0.0, 1.0]),
+        rng.random(3),
+        rng.random(3),
+    ],
+)
+@pytest.mark.parametrize(
+    "point_system_two",
+    [
+        np.zeros((3,)),
+        np.array([0.0, 0.0, -1.0]),
+        rng.random(3),
+        rng.random(3),
+    ],
+)
+def test_generic_free_joint(
+    system_one_class: Union[Type[CosseratRod], Type[RigidBodyBase]],
+    system_two_class: Union[Type[CosseratRod], Type[RigidBodyBase]],
+    point_system_one: np.ndarray,
+    point_system_two: np.ndarray,
+):
+
+    # Origin of the systems
+    origin1 = np.array([0.0, 0.0, 0.0])
+    origin2 = np.array([1.0, 0.0, 0.0])
+
+    system_one = init_system(system_one_class, origin1)
+    system_two = init_system(system_one_class, origin2)
+
+    # Stiffness between points
+    k = 1e8
+
+    # Damping between two points
+    nu = 1
+
+    # System indices
+    system_one_index = -1
+    system_two_index = 0
+
+    # System velocity
+    v1 = np.array([-1, 0, 0]).reshape(3, 1)
+    v2 = v1 * -1
+    system_one.velocity_collection = np.tile(
+        v1, (1, system_one.velocity_collection.shape[1] + 1)
+    )
+    system_two.velocity_collection = np.tile(
+        v2, (1, system_two.velocity_collection.shape[1] + 1)
+    )
+
+    # System angular velocity
+    omega1 = np.array([0.0, 1.0, 0.0]).reshape(3, 1)
+    omega2 = -omega1
+    system_one.omega_collection = np.tile(
+        omega1, (1, system_one.omega_collection.shape[1] + 1)
+    )
+    system_two.omega_collection = np.tile(
+        omega2, (1, system_two.omega_collection.shape[1] + 1)
+    )
+
+    # Verify positions
+    position_system_one = compute_position_of_point(
+        system=system_one, point=point_system_one, index=system_one_index
+    )
+    position_system_two = compute_position_of_point(
+        system=system_two, point=point_system_two, index=system_two_index
+    )
+
+    # this is possible because neither system is rotated yet and still in the initial position
+    # and because the rotation between inertial frame and local frame of the bodies is zero at the start.
+    # The second point can be achieved by choosing the "normal" and "direction" vectors smartly.
+    assert_allclose(
+        position_system_one,
+        system_one.position_collection[:, system_one_index] + point_system_one,
+    )
+    assert_allclose(
+        position_system_two,
+        system_two.position_collection[:, system_two_index] + point_system_two,
+    )
+
+    # Compute the distance between the connection points
+    distance = position_system_two - position_system_one
+    end_distance = np.linalg.norm(distance)
+    if end_distance <= Tolerance.atol():
+        end_distance = 1.0
+
+    # Verify velocities calculated for system one
+    velocity_system_one = compute_velocity_of_point(
+        system=system_one, point=point_system_one, index=system_one_index
+    )
+    if np.array_equal(point_system_one, np.zeros((3, 1))):
+        # for the joint on the node / CoM,
+        # the velocity of the joint needs to be the same as the velocity of the node of system one
+        assert_allclose(
+            velocity_system_one,
+            system_one.velocity_collection[..., system_one_index],
+            atol=Tolerance.atol(),
+        )
+    elif np.array_equal(point_system_one, np.array([0.0, 0.0, 1.0])):
+        # analytical computation of velocity for special case of point_system_two == np.array([0.0, 0.0, 1.0])
+        assert_allclose(
+            velocity_system_one,
+            system_one.velocity_collection[..., system_one_index]
+            + np.array([1.0, 0.0, 0.0]),
+            atol=Tolerance.atol(),
+        )
+
+    # Verify velocities calculated for system two
+    velocity_system_two = compute_velocity_of_point(
+        system=system_two, point=point_system_two, index=system_two_index
+    )
+    if np.array_equal(point_system_two, np.zeros((3, 1))):
+        # for the joint on the node / CoM,
+        # the velocity of the joint needs to be the same as the velocity of the node of system two
+        assert_allclose(
+            velocity_system_two,
+            system_two.velocity_collection[..., system_two_index],
+            atol=Tolerance.atol(),
+        )
+    elif np.array_equal(point_system_two, np.array([0.0, 0.0, -1.0])):
+        # analytical computation of velocity for special case of point_system_two == np.array([0.0, 0.0, -1.0])
+        assert_allclose(
+            velocity_system_two,
+            system_two.velocity_collection[..., system_two_index]
+            + np.array([1.0, 0.0, 0.0]),
+            atol=Tolerance.atol(),
+        )
+
+    # Compute the relative velocity
+    relative_vel = velocity_system_two - velocity_system_one
+
+    # Compute the free joint forces
+    elastic_force = k * distance
+    damping_force = nu * relative_vel
+    external_force = elastic_force + damping_force
+
+    external_force_system_one = external_force
+    external_force_system_two = -1 * external_force
+
+    frjt = GenericSystemTypeFreeJoint(
+        k=k,
+        nu=nu,
+        point_system_one=point_system_one,
+        point_system_two=point_system_two,
+    )
+    frjt.apply_forces(system_one, system_one_index, system_two, system_two_index)
+    frjt.apply_torques(system_one, system_one_index, system_two, system_two_index)
+
+    assert_allclose(
+        system_one.external_forces[..., system_one_index],
+        external_force_system_one,
+        atol=Tolerance.atol(),
+    )
+    assert_allclose(
+        system_two.external_forces[..., system_two_index],
+        external_force_system_two,
+        atol=Tolerance.atol(),
+    )
+
+    # these checks are only possible because the system is not rotated
+    # if system is rotated, first distance between CoM and joint connection point would need to be computed
+    # and torque would need to be rotated into local frame at the end.
+
+    # first check torque on system one
+    if np.array_equal(point_system_one, np.zeros((3, 1))):
+        # for the joint connected at the node / CoM there shouldn't be any torque generated
+        assert_allclose(
+            system_one.external_torques[..., system_one_index],
+            np.zeros((3,)),
+            atol=Tolerance.atol(),
+        )
+    elif np.array_equal(point_system_one, np.array([0.0, 0.0, 1.0])):
+        assert_allclose(
+            system_one.external_torques[..., system_one_index],
+            np.array(
+                [-external_force_system_one[1], external_force_system_one[0], 0.0]
+            ),
+            atol=Tolerance.atol(),
+        )
+    else:
+        assert_allclose(
+            system_one.external_torques[..., system_one_index],
+            np.cross(point_system_one, external_force_system_one),
+            atol=Tolerance.atol(),
+        )
+
+    if np.array_equal(point_system_two, np.zeros((3, 1))):
+        # for the joint connected at the node / CoM there shouldn't be any torque generated
+        assert_allclose(
+            system_two.external_torques[..., system_two_index],
+            np.zeros((3,)),
+            atol=Tolerance.atol(),
+        )
+    elif np.array_equal(point_system_two, np.array([0.0, 0.0, 1.0])):
+        assert_allclose(
+            system_two.external_torques[..., system_two_index],
+            np.array(
+                [-external_force_system_two[1], external_force_system_two[0], 0.0]
+            ),
+            atol=Tolerance.atol(),
+        )
+    else:
+        assert_allclose(
+            system_two.external_torques[..., system_two_index],
+            np.cross(point_system_two, external_force_system_two),
+            atol=Tolerance.atol(),
+        )
diff --git a/tests/test_joint.py b/tests/test_joint.py
index 7c56e73..b12c179 100644
--- a/tests/test_joint.py
+++ b/tests/test_joint.py
@@ -1,19 +1,33 @@
 __doc__ = """ Joint between rods test module """
 
 # System imports
-import numpy as np
-from elastica.joint import FreeJoint, HingeJoint, FixedJoint
+import elastica
+from elastica.experimental.connection_contact_joint.generic_system_type_connection import (
+    compute_position_of_point,
+    compute_velocity_of_point,
+)
+from elastica.joint import (
+    FreeJoint,
+    HingeJoint,
+    FixedJoint,
+)
 from numpy.testing import assert_allclose
-from elastica.utils import Tolerance
+from elastica._rotations import _inv_rotate
+from elastica.rigidbody import Cylinder
 from elastica.rod.cosserat_rod import CosseratRod
-import elastica
+from elastica.utils import Tolerance
 import importlib
+import numpy as np
 import pytest
 from scipy.spatial.transform import Rotation
 
 # TODO: change tests and made them independent of rod, at least assigin hardcoded values for forces and torques
 
 
+# seed random number generator
+rng = np.random.default_rng(0)
+
+
 def test_freejoint():
 
     # Some rod properties. We need them for constructer, they are not used.
@@ -207,12 +221,9 @@ def test_hingejoint():
         rod2.external_forces[..., rod2_index], -1 * contactforce, atol=Tolerance.atol()
     )
 
-    linkdirection = (
-        rod2.position_collection[..., rod2_index + 1]
-        - rod2.position_collection[..., rod2_index]
-    )
-    forcedirection = np.dot(linkdirection, normal1) * normal1
-    torque = -kt * np.cross(linkdirection, forcedirection)
+    system_two_tangent = rod2.director_collection[2, :, rod2_index]
+    force_direction = np.dot(system_two_tangent, normal1) * normal1
+    torque = -kt * np.cross(system_two_tangent, force_direction)
 
     torque_rod1 = -rod1.director_collection[..., rod1_index] @ torque
     torque_rod2 = rod2.director_collection[..., rod2_index] @ torque
@@ -230,7 +241,7 @@ rest_euler_angles = [
     np.array([np.pi / 2, 0.0, 0.0]),
     np.array([0.0, np.pi / 2, 0.0]),
     np.array([0.0, 0.0, np.pi / 2]),
-    2 * np.pi * np.random.random_sample(size=3),
+    2 * np.pi * rng.random(size=3),
 ]
 
 
@@ -361,7 +372,7 @@ def test_fixedjoint(rest_euler_angle):
 
     # compute rotation vectors based on C_22*
     # scipy implementation
-    rot_vec = Rotation.from_matrix(dev_rot).as_rotvec()
+    rot_vec = _inv_rotate(np.dstack([np.eye(3), dev_rot.T])).squeeze()
 
     # rotate rotation vector into inertial frame
     rot_vec_inertial_frame = rod2_director.T @ rot_vec