Merge pull request #24 from GazzolaLab/bugfix-friction

Bugfix friction

elastica/_elastica_numba/_external_forces.py

@@ -342,10 +342,11 @@ def __init__(
         # torque. This coupled with the requirement that the sum of all muscle torques has
         # to be zero results in this condition.
         self.s = np.cumsum(rest_lengths)
+        self.s /= self.s[-1]
 
         if with_spline:
             assert b_coeff.size != 0, "Beta spline coefficient array (t_coeff) is empty"
-            my_spline, ctr_pts, ctr_coeffs = _bspline(b_coeff, base_length)
+            my_spline, ctr_pts, ctr_coeffs = _bspline(b_coeff)
             self.my_spline = my_spline(self.s)
 
         else:

elastica/_elastica_numba/_interaction.py

@@ -478,19 +478,6 @@ def anisotropic_friction(
     slip_function_along_axial_direction = find_slipping_elements(
         velocity_along_axial_direction, slip_velocity_tol
     )
-    kinetic_friction_force_along_axial_direction = -(
-        (1.0 - slip_function_along_axial_direction)
-        * kinetic_mu
-        * plane_response_force_mag
-        * velocity_sign_along_axial_direction
-        * axial_direction
-    )
-    # If rod element does not have any contact with plane, plane cannot apply friction
-    # force on the element. Thus lets set kinetic friction force to 0.0 for the no contact points.
-    kinetic_friction_force_along_axial_direction[..., no_contact_point_idx] = 0.0
-    elements_to_nodes_inplace(
-        kinetic_friction_force_along_axial_direction, external_forces
-    )
 
     # Now rolling kinetic friction
     rolling_direction = _batch_vec_oneD_vec_cross(axial_direction, plane_normal)
@@ -511,17 +498,35 @@ def anisotropic_friction(
     slip_velocity_along_rolling_direction = _batch_product_k_ik_to_ik(
         slip_velocity_mag_along_rolling_direction, rolling_direction
     )
-    slip_velocity_sign_along_rolling_direction = np.sign(
-        slip_velocity_mag_along_rolling_direction
-    )
     slip_function_along_rolling_direction = find_slipping_elements(
         slip_velocity_along_rolling_direction, slip_velocity_tol
     )
+    # Compute unitized total slip velocity vector. We will use this to distribute the weight of the rod in axial
+    # and rolling directions.
+    unitized_total_velocity = (
+        slip_velocity_along_rolling_direction + velocity_along_axial_direction
+    )
+    unitized_total_velocity /= _batch_norm(unitized_total_velocity + 1e-14)
+    # Apply kinetic friction in axial direction.
+    kinetic_friction_force_along_axial_direction = -(
+        (1.0 - slip_function_along_axial_direction)
+        * kinetic_mu
+        * plane_response_force_mag
+        * _batch_dot(unitized_total_velocity, axial_direction)
+        * axial_direction
+    )
+    # If rod element does not have any contact with plane, plane cannot apply friction
+    # force on the element. Thus lets set kinetic friction force to 0.0 for the no contact points.
+    kinetic_friction_force_along_axial_direction[..., no_contact_point_idx] = 0.0
+    elements_to_nodes_inplace(
+        kinetic_friction_force_along_axial_direction, external_forces
+    )
+    # Apply kinetic friction in rolling direction.
     kinetic_friction_force_along_rolling_direction = -(
         (1.0 - slip_function_along_rolling_direction)
         * kinetic_mu_sideways
         * plane_response_force_mag
-        * slip_velocity_sign_along_rolling_direction
+        * _batch_dot(unitized_total_velocity, rolling_direction)
         * rolling_direction
     )
     # If rod element does not have any contact with plane, plane cannot apply friction

elastica/_elastica_numpy/_interaction.py

@@ -333,23 +333,6 @@ def apply_forces(self, system, time=0.0):
         slip_function_along_axial_direction = find_slipping_elements(
             velocity_along_axial_direction, self.slip_velocity_tol
         )
-        kinetic_friction_force_along_axial_direction = -(
-            (1.0 - slip_function_along_axial_direction)
-            * kinetic_mu
-            * plane_response_force_mag
-            * velocity_sign_along_axial_direction
-            * axial_direction
-        )
-        # If rod element does not have any contact with plane, plane cannot apply friction
-        # force on the element. Thus lets set kinetic friction force to 0.0 for the no contact points.
-        kinetic_friction_force_along_axial_direction[..., no_contact_point_idx] = 0.0
-        system.external_forces[..., :-1] += (
-            0.5 * kinetic_friction_force_along_axial_direction
-        )
-        system.external_forces[..., 1:] += (
-            0.5 * kinetic_friction_force_along_axial_direction
-        )
-
         # Now rolling kinetic friction
         rolling_direction = _batch_cross(axial_direction, normal_plane_collection)
         torque_arm = -system.radius * normal_plane_collection
@@ -374,17 +357,43 @@ def apply_forces(self, system, time=0.0):
         slip_velocity_along_rolling_direction = np.einsum(
             "j, ij->ij", slip_velocity_mag_along_rolling_direction, rolling_direction
         )
-        slip_velocity_sign_along_rolling_direction = np.sign(
-            slip_velocity_mag_along_rolling_direction
-        )
         slip_function_along_rolling_direction = find_slipping_elements(
             slip_velocity_along_rolling_direction, self.slip_velocity_tol
         )
+        # Compute unitized total slip velocity vector. We will use this to distribute the weight of the rod in axial
+        # and rolling directions.
+        unitized_total_velocity = (
+            slip_velocity_along_rolling_direction + velocity_along_axial_direction
+        )
+        unitized_total_velocity /= (
+            np.sqrt(
+                np.einsum("ij,ij->j", unitized_total_velocity, unitized_total_velocity)
+            )
+            + 1e-14
+        )
+        # Apply kinetic friction in axial direction.
+        kinetic_friction_force_along_axial_direction = -(
+            (1.0 - slip_function_along_axial_direction)
+            * kinetic_mu
+            * plane_response_force_mag
+            * np.einsum("ij,ij->j", unitized_total_velocity, axial_direction)
+            * axial_direction
+        )
+        # If rod element does not have any contact with plane, plane cannot apply friction
+        # force on the element. Thus lets set kinetic friction force to 0.0 for the no contact points.
+        kinetic_friction_force_along_axial_direction[..., no_contact_point_idx] = 0.0
+        system.external_forces[..., :-1] += (
+            0.5 * kinetic_friction_force_along_axial_direction
+        )
+        system.external_forces[..., 1:] += (
+            0.5 * kinetic_friction_force_along_axial_direction
+        )
+        # Apply kinetic friction in rolling direction.
         kinetic_friction_force_along_rolling_direction = -(
             (1.0 - slip_function_along_rolling_direction)
             * self.kinetic_mu_sideways
             * plane_response_force_mag
-            * slip_velocity_sign_along_rolling_direction
+            * np.einsum("ij,ij->j", unitized_total_velocity, rolling_direction)
             * rolling_direction
         )
         # If rod element does not have any contact with plane, plane cannot apply friction

elastica/utils.py

@@ -5,6 +5,7 @@
 import numpy as np
 from numpy import finfo, float64
 from itertools import islice
+from scipy.interpolate import BSpline
 
 
 # Slower than the python3.8 isqrt implementation for small ints
@@ -185,44 +186,27 @@ def _bspline(t_coeff, l_centerline=1.0):
         which the spline needs to be evaluated.
     """
     # Divide into n_control_pts number of points (n_ctr_pts-1) regions
-    control_pts = l_centerline * np.linspace(0.0, 1.0, t_coeff.shape[0])
+    control_pts = l_centerline * np.linspace(0.0, 1.0, t_coeff.shape[0] - 2)
 
     # Degree of b-spline required. Set to cubic
     degree = 3
 
-    # Update coefficients at the head and tail
-    # Setting it to 0.0 here
-    beta_head = 0.0
-    beta_tail = 0.0
+    return __bspline_impl__(control_pts, t_coeff, degree)
 
-    return __bspline_impl__(control_pts, t_coeff, beta_head, beta_tail, degree)
 
-
-def __bspline_impl__(x_pts, t_c, b_head, b_tail, t_k):
+def __bspline_impl__(x_pts, t_c, degree):
     """"""
-    from scipy.interpolate import BSpline
-
-    # Update the coefficients
-    t_c = np.hstack((b_head, t_c, b_tail))
 
     # Update the knots
-    # You need 2 additional knots for the head and tail control points
-    # You need degree + 1 additional knots to sink into the head and tail for
-    # controlling C^k smoothness. We set it to 0.0
-    n_upd = x_pts.shape[0] + 2 + (t_k + 1)
-
-    # The first and last points are always fixed
-    x_first = x_pts[0]
-    x_last = x_pts[-1]
-    x_pts = np.hstack((x_first, x_pts, x_last))
+    n_upd = t_c.shape[0] + (degree + 1)
 
     # Generate the knots
     knots_updated = np.zeros(n_upd)
     # Fix first degree-1 knots into head
-    knots_updated[: t_k - 1] = x_first
+    knots_updated[:degree] = x_pts[0]
     # Middle knot locations correspond to x_locations
-    knots_updated[t_k - 1 : n_upd - (t_k - 1)] = x_pts
+    knots_updated[degree : n_upd - (degree)] = x_pts
     # Fix first degree-1 knots into tail
-    knots_updated[n_upd - (t_k - 1) :] = x_last
+    knots_updated[n_upd - (degree) :] = x_pts[-1]
 
-    return BSpline(knots_updated, t_c, t_k, extrapolate=False), x_pts, t_c
+    return BSpline(knots_updated, t_c, degree, extrapolate=False), x_pts, t_c

elastica/wrappers/forcing.py

@@ -5,6 +5,7 @@
 Provides the forcing interface to apply forces and torques to rod-like objects
 (external point force, muscle torques, etc).
 """
+from elastica.interaction import AnisotropicFrictionalPlane
 
 
 class Forcing:
@@ -66,6 +67,17 @@ def _finalize(self):
         # to sort _ext_forces_torques.
         self._ext_forces_torques.sort(key=lambda x: x[0])
 
+        # Find if there are any friction plane forcing, if add them to the end of the list,
+        # since friction planes uses external forces.
+        friction_plane_index = []
+        for idx, ext_force_torque in enumerate(self._ext_forces_torques):
+            if isinstance(ext_force_torque[1], AnisotropicFrictionalPlane):
+                friction_plane_index.append(idx)
+
+        # Move to the friction forces to the end of the external force and torques list.
+        for index in friction_plane_index:
+            self._ext_forces_torques.append(self._ext_forces_torques.pop(index))
+
     def __call__(self, time, *args, **kwargs):
         for sys_id, ext_force_torque in self._ext_forces_torques:
             ext_force_torque.apply_forces(self._systems[sys_id], time, *args, **kwargs)