updating sampling logic

- changing sampling to number of points between start and end

src/pybullet_industrial/interpolation.py

@@ -69,7 +69,7 @@ def linear_interpolation(start_point: np.array, end_point: np.array, samples: in
     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 between start and end
         start_orientation (np.array): Start orientation as quaternion
         end_orientation (np.array): End orientation as quaternion
 
@@ -103,7 +103,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 samples between start and end
         clockwise (bool): boolean value indicating if the interpolation is performed clockwise
                             or anticlockwise
 
@@ -148,7 +148,7 @@ def 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 used for the interpolation
-        samples (int): The number of samples used to interpolate
+        samples (int): The number of samples between 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).
         clockwise (bool, optional): The direction of circular travel. Defaults to True.

tests/test_interpolation.py

@@ -14,7 +14,7 @@ def test_linear_interpolation(self):
         # Case 1: Test with both start and end orientations
         start_point = np.array([0.0, 0.0, 0.0])
         end_point = np.array([10.0, 10.0, 10.0])
-        samples = 5
+        samples = 3
         start_orientation = p.getQuaternionFromEuler(
             [0, 0, 0])  # Neutral orientation
         end_orientation = p.getQuaternionFromEuler(
@@ -35,7 +35,7 @@ def test_linear_interpolation(self):
         start_rot = R.from_quat(start_orientation)
         end_rot = R.from_quat(end_orientation)
         rotations = R.from_quat([start_rot.as_quat(), end_rot.as_quat()])
-        t_vals = np.linspace(0, 1, samples)
+        t_vals = np.linspace(0, 1, samples+2)
         slerp_spline = RotationSpline([0, 1], rotations)
         expected_orientations_both = slerp_spline(t_vals).as_quat()
 
@@ -58,7 +58,7 @@ def test_linear_interpolation(self):
 
         # Expected orientations for only start orientation case
         expected_orientations_start_only = np.tile(
-            start_orientation, (samples, 1)).transpose()
+            start_orientation, (samples+2, 1)).transpose()
 
         expected_toolpath_with_start_only = ToolPath(
             positions=expected_positions,
@@ -79,7 +79,7 @@ def test_circular_interpolation(self):
         start_point = np.array([1.0, 0.0, 0.0])
         end_point = np.array([0.0, 1.0, 0.0])
         radius = 1.0
-        samples = 5
+        samples = 3
         axis = 2  # Z-axis
         clockwise = True
         start_orientation = p.getQuaternionFromEuler(
@@ -102,7 +102,7 @@ def test_circular_interpolation(self):
         start_rot = R.from_quat(start_orientation)
         end_rot = R.from_quat(end_orientation)
         rotations = R.from_quat([start_rot.as_quat(), end_rot.as_quat()])
-        t_vals = np.linspace(0, 1, samples)
+        t_vals = np.linspace(0, 1, samples+2)
         slerp_spline = RotationSpline([0, 1], rotations)
         expected_orientations_both = slerp_spline(t_vals).as_quat()
 
@@ -125,7 +125,7 @@ def test_circular_interpolation(self):
 
         # Expected orientations for only start orientation case
         expected_orientations_start_only = np.tile(
-            start_orientation, (samples, 1)).transpose()
+            start_orientation, (samples+2, 1)).transpose()
 
         expected_toolpath_with_start_only = ToolPath(
             positions=expected_positions,
@@ -143,14 +143,40 @@ def test_sample_size(self):
         start_point = np.array([1.0, 0.0, 0.0])
         end_point = np.array([0.0, 1.0, 0.0])
         radius = 1.0
-        samples = 5
+        samples = 0
         axis = 2  # Z-axis
         clockwise = True
         start_orientation = p.getQuaternionFromEuler(
             [0, 0, 0])  # Neutral orientation
         end_orientation = p.getQuaternionFromEuler(
             [0, 0, np.pi / 2])  # 90 degrees rotation around Z-axis
 
+        linear_path = linear_interpolation(
+            start_point, end_point, samples, start_orientation,
+            end_orientation)
+        circular_path = circular_interpolation(
+            start_point, end_point, radius, samples, axis, clockwise,
+            start_orientation, end_orientation)
+
+        expected_position = np.array([[1, 0], [0, 1], [0, 0]])
+        expected_orientation = np.array([
+            [0, 0],
+            [0, 0],
+            [0, 0.70710678],
+            [1, 0.70710678]
+        ])
+
+        expected_toolpath = ToolPath(expected_position, expected_orientation)
+
+        np.testing.assert_array_almost_equal(
+            linear_path.positions, expected_toolpath.positions, decimal=6)
+        np.testing.assert_array_almost_equal(
+            circular_path.positions, expected_toolpath.positions, decimal=6)
+        np.testing.assert_array_almost_equal(
+            linear_path.orientations, expected_toolpath.orientations, decimal=6)
+        np.testing.assert_array_almost_equal(
+            linear_path.orientations, expected_toolpath.orientations, decimal=6)
+
 
 if __name__ == '__main__':
     unittest.main()