Default orientation Bug

.github/workflows/publish_package.yml

@@ -15,8 +15,8 @@ on:
 jobs:
   deploy:
 
-    runs-on: ubuntu-latest
-    
+    runs-on: ubuntu-24.04
+
     env:
       working-directory: src/
 

src/pybullet_industrial/g_code_processor.py

@@ -338,7 +338,7 @@ def __build_precise_path(self, g_com: int):
                     previous_postion = position
 
                 interpolation_steps = total_distance/self.interpolation_precision
-                interpolation_steps = int(np.ceil(interpolation_steps))
+                interpolation_steps = int(np.ceil(interpolation_steps)) + 1
 
         return path
 

src/pybullet_industrial/interpolation.py

@@ -1,10 +1,34 @@
 import numpy as np
 import scipy.interpolate as sci
-import pybullet as p
+from scipy.spatial.transform import Rotation as R, RotationSpline
 
 from pybullet_industrial.toolpath import ToolPath
 
 
+def slerp(start_quat, end_quat, t_vals):
+    """Performs Spherical Linear Interpolation (SLERP) between two quaternions
+
+    Args:
+        start_quat (np.array): The starting quaternion
+        end_quat (np.array): The ending quaternion
+        t_vals (np.array): Array of interpolation parameters (0 <= t <= 1)
+
+    Returns:
+        np.array: The array of interpolated quaternions
+    """
+    start_rot = R.from_quat(start_quat)
+    end_rot = R.from_quat(end_quat)
+
+    # Create the spline (SLERP) object from two quaternions
+    slerp_rotations = R.from_quat([start_rot.as_quat(), end_rot.as_quat()])
+    slerp_spline = RotationSpline([0, 1], slerp_rotations)
+
+    # Perform SLERP interpolation at the time values
+    interpolated_rots = slerp_spline(t_vals)
+
+    return interpolated_rots.as_quat()
+
+
 def build_circular_path(center: np.array, radius: float,
                         min_angle: float, max_angle: float, step_num: int, clockwise: bool = True):
     """Function which builds a circular path
@@ -14,7 +38,7 @@ def build_circular_path(center: np.array, radius: float,
         radius (float): the radius of the circle
         min_angle (float): minimum angle of the circle path
         max_angle (float): maximum angle of the circle path
-        step_num (int): the number of steps between min_angle and max_angle
+        step_num (int): the number of steps including min_angle and max_angle
         clockwise (bool): boolean value indicating if the interpolation is performed clockwise
                           or anticlockwise
 
@@ -25,9 +49,12 @@ def build_circular_path(center: np.array, radius: float,
     circular_path = np.zeros((2, step_num))
     for j in range(step_num):
         if clockwise:
-            path_angle = min_angle-j*(max_angle-min_angle)/step_num
+            path_angle = min_angle - j * \
+                (max_angle - min_angle) / (step_num - 1)
         else:
-            path_angle = min_angle+j*(max_angle-min_angle)/step_num
+            path_angle = min_angle + j * \
+                (max_angle - min_angle) / (step_num - 1)
+
         new_position = center + radius * \
             np.array([np.cos(path_angle), np.sin(path_angle)])
         circular_path[:, j] = new_position
@@ -37,12 +64,12 @@ def build_circular_path(center: np.array, radius: float,
 def linear_interpolation(start_point: np.array, end_point: np.array, samples: int,
                          start_orientation: np.array = None,
                          end_orientation: np.array = None):
-    """Performs a linear interpolation betwenn two points in 3D space
+    """Performs a linear interpolation between two points in 3D space
 
     Args:
         start_point (np.array): The start point of the interpolation
         end_point (np.array): The end point of the interpolation
-        samples (int): The number of samples used to interpolate
+        samples (int): The number of samples including start and end
         start_orientation (np.array): Start orientation as quaternion
         end_orientation (np.array): End orientation as quaternion
 
@@ -52,18 +79,17 @@ def linear_interpolation(start_point: np.array, end_point: np.array, samples: in
     positions = np.linspace(start_point, end_point, num=samples)
     final_path = ToolPath(positions=positions.transpose())
 
-    if start_orientation is not None and end_orientation is not None:
-        start_orientation = p.getEulerFromQuaternion(start_orientation)
-        end_orientation = p.getEulerFromQuaternion(end_orientation)
-        orientations = np.linspace(start_orientation,
-                                   end_orientation, num=samples)
-        final_orientations = []
+    if start_orientation is not None:
+        if end_orientation is None:
+            end_orientation = start_orientation
+
+        # Time values for SLERP interpolation
+        t_vals = np.linspace(0, 1, samples)
 
-        for orientation in orientations:
-            orientation_quat = p.getQuaternionFromEuler(orientation)
-            final_orientations.append(orientation_quat)
+        # Interpolate orientations using SLERP
+        final_orientations = slerp(start_orientation, end_orientation, t_vals)
 
-        final_path.orientations = np.array(final_orientations).transpose()
+        final_path.orientations = final_orientations.transpose()
 
     return final_path
 
@@ -76,7 +102,7 @@ def planar_circular_interpolation(start_point: np.array, end_point: np.array,
         start_point (np.array): The start point of the interpolation
         end_point (np.array): The end point of the interpolation
         radius (float): The radius of the circle
-        samples (int): The number of samples used to interpolate
+        samples (int): The number of including start and end
         clockwise (bool): boolean value indicating if the interpolation is performed clockwise
                             or anticlockwise
 
@@ -115,25 +141,26 @@ def circular_interpolation(start_point: np.array, end_point: np.array,
                            radius: float, samples: int, axis: int = 2,
                            clockwise: bool = True, start_orientation: np.array = None,
                            end_orientation: np.array = None):
-    """AI is creating summary for circular_interpolation
+    """Performs a circular interpolation between two points in 3D space
 
     Args:
         start_point (np.array): The start point of the interpolation
         end_point (np.array): The end point of the interpolation
         radius (float): The radius of the circle used for the interpolation
-        samples (int): The number of samples used to interpolate
+        samples (int): The number of samples including start and end
         axis (int, optional): The axis around which the circle is interpolated.
-                              Defaults to 2 which corresponds to the z-axis (0=x,1=y).
+                              Defaults to 2 which corresponds to the z-axis (0=x, 1=y).
         clockwise (bool, optional): The direction of circular travel. Defaults to True.
         start_orientation (np.array): Start orientation as quaternion
         end_orientation (np.array): End orientation as quaternion
 
     Returns:
         ToolPath: A ToolPath object of the interpolated path
     """
-
     all_axis = [0, 1, 2]
     all_axis.remove(axis)
+
+    # Interpolate positions in the x-y plane
     planar_start_point = np.array(
         [start_point[all_axis[0]], start_point[all_axis[1]]])
     planar_end_point = np.array(
@@ -146,20 +173,20 @@ def circular_interpolation(start_point: np.array, end_point: np.array,
     for i in range(2):
         positions[all_axis[i]] = planar_path[i]
     positions[axis] = np.linspace(start_point[axis], end_point[axis], samples)
+
     final_path = ToolPath(positions=positions)
 
-    if start_orientation is not None and end_orientation is not None:
-        start_orientation = p.getEulerFromQuaternion(start_orientation)
-        end_orientation = p.getEulerFromQuaternion(end_orientation)
-        orientations = np.linspace(start_orientation,
-                                   end_orientation, num=samples)
-        final_orientations = []
+    if start_orientation is not None:
+        if end_orientation is None:
+            end_orientation = start_orientation
+
+        # Time values for SLERP interpolation
+        t_vals = np.linspace(0, 1, samples)
 
-        for orientation in orientations:
-            orientation_quat = p.getQuaternionFromEuler(orientation)
-            final_orientations.append(orientation_quat)
+        # Interpolate orientations using SLERP
+        final_orientations = slerp(start_orientation, end_orientation, t_vals)
 
-        final_path.orientations = np.array(final_orientations).transpose()
+        final_path.orientations = final_orientations.transpose()
 
     return final_path
 

tests/test_agv_robots.py

@@ -146,7 +146,7 @@ def test_standard_position_controller(self):
         distance, angle, target_angle_error = self.robot._calculate_position_error()
 
         # check if the position error is correct
-        self.assertAlmostEqual(distance, 0, delta=0.05)
+        self.assertAlmostEqual(distance, 0, delta=0.06)
 
 
 

tests/test_grippers.py

@@ -84,12 +84,10 @@ def test_grippers(self):
         within_precision = True
 
         path = pi.linear_interpolation(np.array(safepoint1),
-                                       np.array(grippoint1_2), 10)
-        path.orientations = np.transpose([start_orientation_gr] * len(path.orientations[0]))
+                                       np.array(grippoint1_2), 10, start_orientation_gr)
         move_along_path(gripper, path, start_orientation_gr)
-        path = pi.linear_interpolation(
-            np.array(grippoint1_2), np.array(grippoint1), 10)
-        path.orientations = np.transpose([start_orientation_gr] * len(path.orientations[0]))
+        path = pi.circular_interpolation(
+            start_point=np.array(grippoint1_2), end_point=np.array(grippoint1), samples=10, axis=0, radius=10, start_orientation=start_orientation_gr)
         move_along_path(gripper, path)
         for _ in range(25):
             p.stepSimulation()