From 06611d25cea2b4394d2c0822f5b130fb17ad3c0e Mon Sep 17 00:00:00 2001 From: philipp1604 <122354024+philipp1604@users.noreply.github.com> Date: Fri, 25 Oct 2024 11:40:16 +0200 Subject: [PATCH] Changin back sampling size - sampling size should always be at least 2, as start and end point always have to be part of the samples - some classes like test_endeffector use the sampling size wrong, but without changing there test methods, this bug can not be fixed --- src/pybullet_industrial/g_code_processor.py | 2 +- src/pybullet_industrial/interpolation.py | 10 ++++------ tests/test_interpolation.py | 16 ++++++++-------- 3 files changed, 13 insertions(+), 15 deletions(-) diff --git a/src/pybullet_industrial/g_code_processor.py b/src/pybullet_industrial/g_code_processor.py index 7e46759..fab0a66 100644 --- a/src/pybullet_industrial/g_code_processor.py +++ b/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)) + 2 return path diff --git a/src/pybullet_industrial/interpolation.py b/src/pybullet_industrial/interpolation.py index 465011f..28d063a 100644 --- a/src/pybullet_industrial/interpolation.py +++ b/src/pybullet_industrial/interpolation.py @@ -38,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 @@ -69,14 +69,13 @@ 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 between start and end + 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 Returns: ToolPath: A ToolPath object of the interpolated path """ - samples = samples + 2 positions = np.linspace(start_point, end_point, num=samples) final_path = ToolPath(positions=positions.transpose()) @@ -103,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 between start and end + samples (int): The number of including start and end clockwise (bool): boolean value indicating if the interpolation is performed clockwise or anticlockwise @@ -148,7 +147,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 between start and end + 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). clockwise (bool, optional): The direction of circular travel. Defaults to True. @@ -158,7 +157,6 @@ def circular_interpolation(start_point: np.array, end_point: np.array, Returns: ToolPath: A ToolPath object of the interpolated path """ - samples = samples + 2 all_axis = [0, 1, 2] all_axis.remove(axis) diff --git a/tests/test_interpolation.py b/tests/test_interpolation.py index 49a42a3..d982cb3 100644 --- a/tests/test_interpolation.py +++ b/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 = 3 + samples = 5 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+2) + t_vals = np.linspace(0, 1, samples) 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+2, 1)).transpose() + start_orientation, (samples, 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 = 3 + samples = 5 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+2) + t_vals = np.linspace(0, 1, samples) 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+2, 1)).transpose() + start_orientation, (samples, 1)).transpose() expected_toolpath_with_start_only = ToolPath( positions=expected_positions, @@ -139,11 +139,11 @@ def test_circular_interpolation(self): result_with_only_start_orientation.orientations, expected_toolpath_with_start_only.orientations, decimal=6) - def test_sample_size(self): + def test_zero_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 = 0 + samples = 2 axis = 2 # Z-axis clockwise = True start_orientation = p.getQuaternionFromEuler(