From f54e2a6c2c4fda858ec720d789642784939422b3 Mon Sep 17 00:00:00 2001 From: Philipp Date: Mon, 17 Jul 2023 14:24:37 +0200 Subject: [PATCH 1/9] Implentation of the linspace for the orientation - change of the orientation of the the interpolation --- src/pybullet_industrial/interpolation.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/pybullet_industrial/interpolation.py b/src/pybullet_industrial/interpolation.py index 76aa91f..25bc2c1 100644 --- a/src/pybullet_industrial/interpolation.py +++ b/src/pybullet_industrial/interpolation.py @@ -33,19 +33,26 @@ def build_circular_path(center: np.array, radius: float, return circular_path -def linear_interpolation(start_point: np.array, end_point: np.array, samples: int): +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 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 + 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 """ final_path = np.linspace(start_point, end_point, num=samples) - return ToolPath(final_path.transpose()) + final_orientations = np.linspace( + start_orientation, end_orientation, num=samples) + return ToolPath(final_path.transpose(), final_orientations.transpose()) def planar_circular_interpolation(start_point: np.array, end_point: np.array, From 3a60502f41b2d4ba066b2a118ce56d8c302ab8c0 Mon Sep 17 00:00:00 2001 From: Philipp Date: Tue, 18 Jul 2023 16:25:11 +0200 Subject: [PATCH 2/9] Linspace as Interoplation of Orientations - The same convention as for linear point interpolation was implemented for the interpolation of the orientations --- examples/interpolation.py | 161 +++++++++++++++++++++++ examples/test_interpolation.py | 14 ++ src/pybullet_industrial/interpolation.py | 11 +- 3 files changed, 184 insertions(+), 2 deletions(-) create mode 100644 examples/interpolation.py create mode 100644 examples/test_interpolation.py diff --git a/examples/interpolation.py b/examples/interpolation.py new file mode 100644 index 0000000..882ce7b --- /dev/null +++ b/examples/interpolation.py @@ -0,0 +1,161 @@ +import numpy as np +import scipy.interpolate as sci +from scipy.spatial.transform import Rotation + +from pybullet_industrial.toolpath import ToolPath + + +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 + + Args: + center (np.array): the center of the circle + 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 + clockwise (bool): boolean value indicating if the interpolation is performed clockwise + or anticlockwise + + Returns: + np.array: array of 2 dimensional path points + """ + + 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 + else: + path_angle = min_angle+j*(max_angle-min_angle)/step_num + new_position = center + radius * \ + np.array([np.cos(path_angle), np.sin(path_angle)]) + circular_path[:, j] = new_position + return circular_path + + +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 + + 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 + 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 + """ + final_path = np.linspace(start_point, end_point, num=samples) + + final_orientations = np.linspace(start_orientation, + end_orientation, num=samples) + + return ToolPath(final_path.transpose(), final_orientations.transpose()) + + +def planar_circular_interpolation(start_point: np.array, end_point: np.array, + radius: float, samples: int, clockwise: bool = True): + """Helper function which performs a circular interpolation in the x-y plane + + 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 + samples (int): The number of samples used to interpolate + clockwise (bool): boolean value indicating if the interpolation is performed clockwise + or anticlockwise + + Returns: + np.array: The interpolated path + """ + connecting_line = end_point-start_point + distance_between_points = np.linalg.norm(connecting_line) + if radius <= distance_between_points/2: + raise ValueError("The radius needs to be at least " + + str(distance_between_points/2)) + + center_distance_from_connecting_line = np.sqrt( + radius**2-distance_between_points**2/4) + + if clockwise: + orthogonal_vector = np.array( + [connecting_line[1], -1*connecting_line[0]]) + else: + orthogonal_vector = np.array( + [-1*connecting_line[1], connecting_line[0]]) + + circle_center = start_point+connecting_line/2+center_distance_from_connecting_line * \ + orthogonal_vector/np.linalg.norm(orthogonal_vector) + + angle_range = np.arccos(center_distance_from_connecting_line/radius)*2 + initial_angle = np.arctan2( + start_point[1]-circle_center[1], start_point[0]-circle_center[0]) + + planar_path = build_circular_path( + circle_center, radius, initial_angle, initial_angle+angle_range, samples, clockwise) + return planar_path + + +def circular_interpolation(start_point: np.array, end_point: np.array, + radius: float, samples: int, axis: int = 2, clockwise: bool = True): + """AI is creating summary for circular_interpolation + + 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 + 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. + + Returns: + ToolPath: A ToolPath object of the interpolated path + """ + + all_axis = [0, 1, 2] + all_axis.remove(axis) + planar_start_point = np.array( + [start_point[all_axis[0]], start_point[all_axis[1]]]) + planar_end_point = np.array( + [end_point[all_axis[0]], end_point[all_axis[1]]]) + + planar_path = planar_circular_interpolation( + planar_start_point, planar_end_point, radius, samples, clockwise) + + path = np.zeros((3, samples)) + for i in range(2): + path[all_axis[i]] = planar_path[i] + path[axis] = np.linspace(start_point[axis], end_point[axis], samples) + return ToolPath(path) + + +def spline_interpolation(points: np.array, samples: int): + """Interpolates between a number of points in cartesian space. + + Args: + points (np.array(3,n)): A 3 dimensional array whith each dimension containing + subsequent positions. + samples (int): The number of samples used to interpolate + + Returns: + ToolPath: A ToolPath object of the interpolated path + """ + s = np.linspace(0, 1, len(points[0])) + + path = np.zeros((3, samples)) + cs_x = sci.CubicSpline(s, points[0]) + cs_y = sci.CubicSpline(s, points[1]) + cs_z = sci.CubicSpline(s, points[2]) + + cs_s = np.linspace(0, 1, samples) + path[0] = cs_x(cs_s) + path[1] = cs_y(cs_s) + path[2] = cs_z(cs_s) + + return ToolPath(path) diff --git a/examples/test_interpolation.py b/examples/test_interpolation.py new file mode 100644 index 0000000..fe63cab --- /dev/null +++ b/examples/test_interpolation.py @@ -0,0 +1,14 @@ +import pybullet as p +import numpy as np +from interpolation import linear_interpolation + +if __name__ == "__main__": + + x1 = [0, 0, 0] + x2 = [10, 0, 0] + o1 = p.getQuaternionFromEuler([0, 0, 0]) + o2 = p.getQuaternionFromEuler([(-np.pi/2), 0, 0]) + path = linear_interpolation(x1, x2, 10, o1, o2) + + for position, orientation, _ in path: + print(p.getEulerFromQuaternion(orientation)) diff --git a/src/pybullet_industrial/interpolation.py b/src/pybullet_industrial/interpolation.py index 25bc2c1..f10980d 100644 --- a/src/pybullet_industrial/interpolation.py +++ b/src/pybullet_industrial/interpolation.py @@ -1,5 +1,6 @@ import numpy as np import scipy.interpolate as sci +from scipy.spatial.transform import Rotation from pybullet_industrial.toolpath import ToolPath @@ -50,8 +51,14 @@ def linear_interpolation(start_point: np.array, end_point: np.array, ToolPath: A ToolPath object of the interpolated path """ final_path = np.linspace(start_point, end_point, num=samples) - final_orientations = np.linspace( - start_orientation, end_orientation, num=samples) + + start_rotation = Rotation.from_quat(start_orientation) + end_rotation = Rotation.from_quat(end_orientation) + + # Perform quaternion interpolation + interpolated_rotations = start_rotation.interpolate( + end_rotation, samples) + return ToolPath(final_path.transpose(), final_orientations.transpose()) From 4393333ef71ac7950e8b489cbfbba45d4922df44 Mon Sep 17 00:00:00 2001 From: Philipp Date: Wed, 26 Jul 2023 19:05:34 +0200 Subject: [PATCH 3/9] Interpolation Demonstration Class - Implementation of the different interpolation paths --- examples/interpolation.py | 53 +++++++++----- examples/test_interpolation.py | 127 +++++++++++++++++++++++++++++++-- 2 files changed, 156 insertions(+), 24 deletions(-) diff --git a/examples/interpolation.py b/examples/interpolation.py index 882ce7b..f923843 100644 --- a/examples/interpolation.py +++ b/examples/interpolation.py @@ -36,8 +36,7 @@ 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, - ): + end_orientation: np.array = None): """Performs a linear interpolation betwenn two points in 3D space Args: @@ -50,12 +49,16 @@ def linear_interpolation(start_point: np.array, end_point: np.array, Returns: ToolPath: A ToolPath object of the interpolated path """ - final_path = np.linspace(start_point, end_point, num=samples) + positions = np.linspace(start_point, end_point, num=samples) - final_orientations = np.linspace(start_orientation, - end_orientation, num=samples) + final_path = ToolPath(positions=positions.transpose()) - return ToolPath(final_path.transpose(), final_orientations.transpose()) + if start_orientation is not None and end_orientation is not None: + orientations = np.linspace(start_orientation, + end_orientation, num=samples) + final_path.orientations = orientations.transpose() + + return final_path def planar_circular_interpolation(start_point: np.array, end_point: np.array, @@ -102,7 +105,8 @@ def planar_circular_interpolation(start_point: np.array, end_point: np.array, def circular_interpolation(start_point: np.array, end_point: np.array, - radius: float, samples: int, axis: int = 2, clockwise: bool = True): + 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 Args: @@ -128,14 +132,22 @@ def circular_interpolation(start_point: np.array, end_point: np.array, planar_path = planar_circular_interpolation( planar_start_point, planar_end_point, radius, samples, clockwise) - path = np.zeros((3, samples)) + positions = np.zeros((3, samples)) for i in range(2): - path[all_axis[i]] = planar_path[i] - path[axis] = np.linspace(start_point[axis], end_point[axis], samples) - return ToolPath(path) + 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: + orientations = np.linspace(start_orientation, + end_orientation, num=samples) + final_path.orientations = orientations.transpose() + + return final_path -def spline_interpolation(points: np.array, samples: int): +def spline_interpolation(points: np.array, samples: int, start_orientation: np.array = None, + end_orientation: np.array = None): """Interpolates between a number of points in cartesian space. Args: @@ -148,14 +160,21 @@ def spline_interpolation(points: np.array, samples: int): """ s = np.linspace(0, 1, len(points[0])) - path = np.zeros((3, samples)) + positions = np.zeros((3, samples)) cs_x = sci.CubicSpline(s, points[0]) cs_y = sci.CubicSpline(s, points[1]) cs_z = sci.CubicSpline(s, points[2]) cs_s = np.linspace(0, 1, samples) - path[0] = cs_x(cs_s) - path[1] = cs_y(cs_s) - path[2] = cs_z(cs_s) + positions[0] = cs_x(cs_s) + positions[1] = cs_y(cs_s) + positions[2] = cs_z(cs_s) + + final_path = ToolPath(positions=positions) + + if start_orientation is not None and end_orientation is not None: + orientations = np.linspace(start_orientation, + end_orientation, num=samples) + final_path.orientations = orientations.transpose() - return ToolPath(path) + return final_path diff --git a/examples/test_interpolation.py b/examples/test_interpolation.py index fe63cab..3bd1d2e 100644 --- a/examples/test_interpolation.py +++ b/examples/test_interpolation.py @@ -1,14 +1,127 @@ +import os import pybullet as p import numpy as np +import pybullet_data +import pybullet_industrial as pi from interpolation import linear_interpolation +from interpolation import circular_interpolation +from interpolation import spline_interpolation +from pybullet_industrial import ToolPath + + +def run_elementary_operations(elementary_operations: list): + for operation in elementary_operations: + operation() + for _ in range(200): + p.stepSimulation() -if __name__ == "__main__": - x1 = [0, 0, 0] - x2 = [10, 0, 0] - o1 = p.getQuaternionFromEuler([0, 0, 0]) - o2 = p.getQuaternionFromEuler([(-np.pi/2), 0, 0]) - path = linear_interpolation(x1, x2, 10, o1, o2) +def create_movement_operations(path: ToolPath, robot: pi.RobotBase): + """Returns a list of elementary operations to move the robot based + on a given tool path and active endeffector. + + Args: + path(ToolPath): input tool path + + Returns: + elementary_operations(list): elementary operations to move robot + """ + elementary_operations = [] for position, orientation, _ in path: - print(p.getEulerFromQuaternion(orientation)) + elementary_operations.append( + lambda i=position, j=orientation: + robot.set_endeffector_pose(i, j)) + + return elementary_operations + + +if __name__ == "__main__": + + dirname = os.path.dirname(__file__) + urdf_file1 = os.path.join(dirname, + 'robot_descriptions', 'comau_nj290_robot.urdf') + urdf_file3 = os.path.join(dirname, + 'Objects', 'FoFa', 'FoFa.urdf') + + pysics_client = p.connect(p.GUI, options='--background_color_red=1 ' + + '--background_color_green=1 ' + + '--background_color_blue=1') + p.setPhysicsEngineParameter(numSolverIterations=10000) + p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) + p.setAdditionalSearchPath(pybullet_data.getDataPath()) + p.setGravity(0, 0, -10) + p.loadURDF(urdf_file3, [-2, 5, 0], useFixedBase=True, globalScaling=0.001) + + test_robot = pi.RobotBase(urdf_file1, [0, 0, 0], [0, 0, 0, 1]) + + test_robot.set_joint_position(({'q2': np.deg2rad(-15.0), + 'q3': np.deg2rad(-90.0)})) + for _ in range(100): + p.stepSimulation() + + # Setting the points + pt_start, orn_start = test_robot.get_endeffector_pose() + pt_1 = [2, -1.5, 1.5] + orn_1 = p.getQuaternionFromEuler([-np.pi/2, 0, 0]) + pt_2 = [2.3, 0, 2.4] + orn_2 = p.getQuaternionFromEuler([-3*np.pi/2, 0, 0]) + + # Seting the interpolation parameters + samples = 100 + clockwise = True # Circular Direction + radius = 2 + axis = 2 + + # Path 1: Linear path + linear_path = linear_interpolation(start_point=pt_start, end_point=pt_1, + samples=samples, + start_orientation=orn_start, + end_orientation=orn_1) + elementary_operations = create_movement_operations(linear_path, test_robot) + run_elementary_operations(elementary_operations) + + # Path 2: Linear path + linear_path = linear_interpolation(start_point=pt_1, end_point=pt_2, + samples=samples, + start_orientation=orn_1, + end_orientation=orn_2) + + elementary_operations = create_movement_operations(linear_path, test_robot) + run_elementary_operations(elementary_operations) + + # Path 3: Circular path + circular_path = circular_interpolation(start_point=pt_2, end_point=pt_1, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=orn_2, + end_orientation=orn_1) + elementary_operations = create_movement_operations( + circular_path, test_robot) + run_elementary_operations(elementary_operations) + + # Path 4: Circular path + clockwise = False + axis = 1 + circular_path = circular_interpolation(start_point=pt_1, end_point=pt_2, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=orn_1, + end_orientation=orn_2) + elementary_operations = create_movement_operations( + circular_path, test_robot) + run_elementary_operations(elementary_operations) + + # Path 5: Spline path + spline_points = [] + spline_pt_1 = [1, -1.5, 2.4] + spline_pt_2 = [2, -1.5, 2.4] + spline_pt_3 = [2, -1.5, 1.5] + spline_points = [spline_pt_1, spline_pt_2, spline_pt_3] + + spline_path = spline_interpolation(spline_points, samples=samples, + start_orientation=orn_1, + end_orientation=orn_2) + elementary_operations = create_movement_operations( + spline_path, test_robot) + run_elementary_operations(elementary_operations) From 795cffea2b4c2fe96b68e2af9eaa683b94a0572e Mon Sep 17 00:00:00 2001 From: Philipp Date: Thu, 3 Aug 2023 22:43:04 +0200 Subject: [PATCH 4/9] Interpolation Dem -Demonstration of the interpolation class --- examples/interpolation_dem.py | 216 +++++++++++++++++++++++++++++++++ examples/test_interpolation.py | 127 ------------------- 2 files changed, 216 insertions(+), 127 deletions(-) create mode 100644 examples/interpolation_dem.py delete mode 100644 examples/test_interpolation.py diff --git a/examples/interpolation_dem.py b/examples/interpolation_dem.py new file mode 100644 index 0000000..bb4088d --- /dev/null +++ b/examples/interpolation_dem.py @@ -0,0 +1,216 @@ +import os +import pybullet as p +import numpy as np +import pybullet_data +import pybullet_industrial as pi +from interpolation import linear_interpolation +from interpolation import circular_interpolation +from interpolation import spline_interpolation +from pybullet_industrial import ToolPath + + +def run_elementary_operations(elementary_operations: list): + for operation in elementary_operations: + operation() + for _ in range(200): + p.stepSimulation() + + +def create_movement_operations(path: ToolPath, robot: pi.RobotBase): + """Returns a list of elementary operations to move the robot based + on a given tool path and active endeffector. + + Args: + path(ToolPath): input tool path + + Returns: + elementary_operations(list): elementary operations to move robot + """ + elementary_operations = [] + + for position, orientation, _ in path: + elementary_operations.append( + lambda i=position, j=orientation: + robot.set_endeffector_pose(i, j)) + + return elementary_operations + + +if __name__ == "__main__": + + dirname = os.path.dirname(__file__) + urdf_file1 = os.path.join(dirname, + 'robot_descriptions', 'comau_nj290_robot.urdf') + urdf_file3 = os.path.join(dirname, + 'Objects', 'FoFa', 'FoFa.urdf') + + pysics_client = p.connect(p.GUI, options='--background_color_red=1 ' + + '--background_color_green=1 ' + + '--background_color_blue=1') + p.setPhysicsEngineParameter(numSolverIterations=10000) + p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) + p.setAdditionalSearchPath(pybullet_data.getDataPath()) + p.setGravity(0, 0, -10) + p.loadURDF(urdf_file3, [-2, 5, 0], useFixedBase=True, globalScaling=0.001) + + test_robot = pi.RobotBase(urdf_file1, [0, 0, 0], [0, 0, 0, 1]) + + test_robot.set_joint_position(({'q2': np.deg2rad(-15.0), + 'q3': np.deg2rad(-90.0)})) + for _ in range(100): + p.stepSimulation() + + # Setting the points + pt_start, orn_start = test_robot.get_endeffector_pose() + pt_1 = [1.5, -2, 2] + orn_1 = p.getQuaternionFromEuler([-np.pi/2, 0, 0]) + pt_2 = [1.5, 0, 2] + orn_2 = p.getQuaternionFromEuler([-3*np.pi/2, 0, 0]) + pt_3 = [1.5, -1, 2] + pt_4 = [2, -1, 2] + pt_5 = [2.5, -1, 2] + + # Setting the colors + + color_1 = [1, 0, 0] + color_2 = [0, 1, 0] + color_3 = [0, 0, 1] + color_4 = [1, 0, 1] + color_5 = [1, 1, 0] + color_6 = [0, 1, 1] + color_7 = [1, 1, 1] + color_8 = [0.5, 0.5, 0.5] + + # Seting the interpolation parameters + samples = 100 + clockwise = True # Circular Direction + radius = 0.6 + axis = 2 + + # Setting position + linear_path = linear_interpolation(start_point=pt_start, end_point=pt_2, + samples=samples, + start_orientation=orn_start, + end_orientation=orn_2) + elementary_operations = create_movement_operations(linear_path, test_robot) + run_elementary_operations(elementary_operations) + + # Path 1: Linear path + linear_path = linear_interpolation(start_point=pt_2, end_point=pt_1, + samples=samples, + start_orientation=orn_2, + end_orientation=orn_1) + elementary_operations = create_movement_operations(linear_path, test_robot) + linear_path.draw(color=color_1) + run_elementary_operations(elementary_operations) + + # # Path 2: Linear path + # linear_path = linear_interpolation(start_point=pt_1, end_point=pt_2, + # samples=samples, + # start_orientation=orn_1, + # end_orientation=orn_2) + + # elementary_operations = create_movement_operations(linear_path, test_robot) + # linear_path.draw(color=[1, 1, 1]) + # run_elementary_operations(elementary_operations) + + # Path 3: Circular path + circular_path = circular_interpolation(start_point=pt_1, end_point=pt_3, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=orn_1, + end_orientation=orn_1) + elementary_operations = create_movement_operations( + circular_path, test_robot) + circular_path.draw(color=color_2) + run_elementary_operations(elementary_operations) + + # Path 4: Circular path + clockwise = False + circular_path = circular_interpolation(start_point=pt_3, end_point=pt_2, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=orn_1, + end_orientation=orn_1) + elementary_operations = create_movement_operations( + circular_path, test_robot) + circular_path.draw(color=color_3) + run_elementary_operations(elementary_operations) + + # Path 4: Circular path + axis = 0 + circular_path = circular_interpolation(start_point=pt_2, end_point=pt_3, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=orn_1, + end_orientation=orn_1) + elementary_operations = create_movement_operations( + circular_path, test_robot) + circular_path.draw(color=color_4) + run_elementary_operations(elementary_operations) + + # Path 4: Circular path + axis = 0 + clockwise = True + circular_path = circular_interpolation(start_point=pt_3, end_point=pt_1, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=orn_1, + end_orientation=orn_1) + elementary_operations = create_movement_operations( + circular_path, test_robot) + circular_path.draw(color=color_5) + run_elementary_operations(elementary_operations) + + # Path 1: Linear path + linear_path = linear_interpolation(start_point=pt_1, end_point=pt_3, + samples=samples, + start_orientation=orn_1, + end_orientation=orn_1) + elementary_operations = create_movement_operations(linear_path, test_robot) + run_elementary_operations(elementary_operations) + + # Path 4: Circular path + axis = 1 + clockwise = True + circular_path = circular_interpolation(start_point=pt_3, end_point=pt_4, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=orn_1, + end_orientation=orn_1) + elementary_operations = create_movement_operations( + circular_path, test_robot) + circular_path.draw(color=color_6) + run_elementary_operations(elementary_operations) + + # Path 4: Circular path + axis = 1 + clockwise = False + circular_path = circular_interpolation(start_point=pt_4, end_point=pt_5, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=orn_1, + end_orientation=orn_1) + elementary_operations = create_movement_operations( + circular_path, test_robot) + circular_path.draw(color=color_7) + run_elementary_operations(elementary_operations) + + # Path 1: Linear path + linear_path = linear_interpolation(start_point=pt_5, end_point=pt_1, + samples=samples, + start_orientation=orn_1, + end_orientation=orn_1) + elementary_operations = create_movement_operations(linear_path, test_robot) + run_elementary_operations(elementary_operations) + + # Path 1: Spline path + + spline_path = spline_interpolation( + [pt_1, pt_4, pt_2], samples=samples) + spline_path.draw(color=color_8, pose=True) + spline_path.orientations = np.transpose([orn_1] + * len(spline_path.orientations[0])) + elementary_operations = create_movement_operations( + spline_path, test_robot) + run_elementary_operations(elementary_operations) diff --git a/examples/test_interpolation.py b/examples/test_interpolation.py deleted file mode 100644 index 3bd1d2e..0000000 --- a/examples/test_interpolation.py +++ /dev/null @@ -1,127 +0,0 @@ -import os -import pybullet as p -import numpy as np -import pybullet_data -import pybullet_industrial as pi -from interpolation import linear_interpolation -from interpolation import circular_interpolation -from interpolation import spline_interpolation -from pybullet_industrial import ToolPath - - -def run_elementary_operations(elementary_operations: list): - for operation in elementary_operations: - operation() - for _ in range(200): - p.stepSimulation() - - -def create_movement_operations(path: ToolPath, robot: pi.RobotBase): - """Returns a list of elementary operations to move the robot based - on a given tool path and active endeffector. - - Args: - path(ToolPath): input tool path - - Returns: - elementary_operations(list): elementary operations to move robot - """ - elementary_operations = [] - - for position, orientation, _ in path: - elementary_operations.append( - lambda i=position, j=orientation: - robot.set_endeffector_pose(i, j)) - - return elementary_operations - - -if __name__ == "__main__": - - dirname = os.path.dirname(__file__) - urdf_file1 = os.path.join(dirname, - 'robot_descriptions', 'comau_nj290_robot.urdf') - urdf_file3 = os.path.join(dirname, - 'Objects', 'FoFa', 'FoFa.urdf') - - pysics_client = p.connect(p.GUI, options='--background_color_red=1 ' + - '--background_color_green=1 ' + - '--background_color_blue=1') - p.setPhysicsEngineParameter(numSolverIterations=10000) - p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) - p.setAdditionalSearchPath(pybullet_data.getDataPath()) - p.setGravity(0, 0, -10) - p.loadURDF(urdf_file3, [-2, 5, 0], useFixedBase=True, globalScaling=0.001) - - test_robot = pi.RobotBase(urdf_file1, [0, 0, 0], [0, 0, 0, 1]) - - test_robot.set_joint_position(({'q2': np.deg2rad(-15.0), - 'q3': np.deg2rad(-90.0)})) - for _ in range(100): - p.stepSimulation() - - # Setting the points - pt_start, orn_start = test_robot.get_endeffector_pose() - pt_1 = [2, -1.5, 1.5] - orn_1 = p.getQuaternionFromEuler([-np.pi/2, 0, 0]) - pt_2 = [2.3, 0, 2.4] - orn_2 = p.getQuaternionFromEuler([-3*np.pi/2, 0, 0]) - - # Seting the interpolation parameters - samples = 100 - clockwise = True # Circular Direction - radius = 2 - axis = 2 - - # Path 1: Linear path - linear_path = linear_interpolation(start_point=pt_start, end_point=pt_1, - samples=samples, - start_orientation=orn_start, - end_orientation=orn_1) - elementary_operations = create_movement_operations(linear_path, test_robot) - run_elementary_operations(elementary_operations) - - # Path 2: Linear path - linear_path = linear_interpolation(start_point=pt_1, end_point=pt_2, - samples=samples, - start_orientation=orn_1, - end_orientation=orn_2) - - elementary_operations = create_movement_operations(linear_path, test_robot) - run_elementary_operations(elementary_operations) - - # Path 3: Circular path - circular_path = circular_interpolation(start_point=pt_2, end_point=pt_1, - samples=samples, axis=axis, - clockwise=clockwise, radius=radius, - start_orientation=orn_2, - end_orientation=orn_1) - elementary_operations = create_movement_operations( - circular_path, test_robot) - run_elementary_operations(elementary_operations) - - # Path 4: Circular path - clockwise = False - axis = 1 - circular_path = circular_interpolation(start_point=pt_1, end_point=pt_2, - samples=samples, axis=axis, - clockwise=clockwise, radius=radius, - start_orientation=orn_1, - end_orientation=orn_2) - elementary_operations = create_movement_operations( - circular_path, test_robot) - run_elementary_operations(elementary_operations) - - # Path 5: Spline path - spline_points = [] - spline_pt_1 = [1, -1.5, 2.4] - spline_pt_2 = [2, -1.5, 2.4] - spline_pt_3 = [2, -1.5, 1.5] - spline_points = [spline_pt_1, spline_pt_2, spline_pt_3] - - spline_path = spline_interpolation(spline_points, samples=samples, - start_orientation=orn_1, - end_orientation=orn_2) - elementary_operations = create_movement_operations( - spline_path, test_robot) - run_elementary_operations(elementary_operations) From 549ac243a36975f485c8d6af5aab27fc90ec792a Mon Sep 17 00:00:00 2001 From: Philipp Date: Thu, 3 Aug 2023 23:45:02 +0200 Subject: [PATCH 5/9] Upddate -uppdate of the new interpolation_dem --- examples/gripping.py | 2 +- examples/interpolation_dem.py | 214 +++++++++++++++++----------------- 2 files changed, 109 insertions(+), 107 deletions(-) diff --git a/examples/gripping.py b/examples/gripping.py index 36eeb91..d3cacaf 100644 --- a/examples/gripping.py +++ b/examples/gripping.py @@ -1,5 +1,5 @@ """This example demonstrates how gripping can be done with the pybullet_industrial library. - It uses the Comau NJ290 robot and a two types of grippers, + It uses the Comau NJ290 robot and a two types of grippers, a vacuum gripper and a parallel gripper. The vacuum gripper is used to pick up a cube and move it to a new position. The parallel gripper is then used to return the cube to its original position. diff --git a/examples/interpolation_dem.py b/examples/interpolation_dem.py index bb4088d..ac2bc82 100644 --- a/examples/interpolation_dem.py +++ b/examples/interpolation_dem.py @@ -9,6 +9,11 @@ from pybullet_industrial import ToolPath +def simulate_path(path: ToolPath, robot: pi.RobotBase): + elementary_operations = create_movement_operations(path, robot) + run_elementary_operations(elementary_operations) + + def run_elementary_operations(elementary_operations: list): for operation in elementary_operations: operation() @@ -63,13 +68,18 @@ def create_movement_operations(path: ToolPath, robot: pi.RobotBase): # Setting the points pt_start, orn_start = test_robot.get_endeffector_pose() pt_1 = [1.5, -2, 2] - orn_1 = p.getQuaternionFromEuler([-np.pi/2, 0, 0]) pt_2 = [1.5, 0, 2] - orn_2 = p.getQuaternionFromEuler([-3*np.pi/2, 0, 0]) pt_3 = [1.5, -1, 2] pt_4 = [2, -1, 2] pt_5 = [2.5, -1, 2] + # Setting the orientations + orn_1 = p.getQuaternionFromEuler([-np.pi/2, 0, 0]) + orn_2 = p.getQuaternionFromEuler([-3*np.pi/2, 0, 0]) + orn_3 = p.getQuaternionFromEuler([0, np.pi/2, 0]) + orn_4 = p.getQuaternionFromEuler([0, 0, np.pi/2]) + test_orientations = [orn_2, orn_3, orn_4] + # Setting the colors color_1 = [1, 0, 0] @@ -88,129 +98,121 @@ def create_movement_operations(path: ToolPath, robot: pi.RobotBase): axis = 2 # Setting position - linear_path = linear_interpolation(start_point=pt_start, end_point=pt_2, + linear_path = linear_interpolation(start_point=pt_start, end_point=pt_4, samples=samples, start_orientation=orn_start, - end_orientation=orn_2) - elementary_operations = create_movement_operations(linear_path, test_robot) - run_elementary_operations(elementary_operations) - - # Path 1: Linear path - linear_path = linear_interpolation(start_point=pt_2, end_point=pt_1, - samples=samples, - start_orientation=orn_2, end_orientation=orn_1) elementary_operations = create_movement_operations(linear_path, test_robot) - linear_path.draw(color=color_1) run_elementary_operations(elementary_operations) - # # Path 2: Linear path - # linear_path = linear_interpolation(start_point=pt_1, end_point=pt_2, - # samples=samples, - # start_orientation=orn_1, - # end_orientation=orn_2) + # Test different orientations + for test_orn in test_orientations: + i = orn_1 + j = test_orn + for _ in range(2): + path = linear_interpolation(start_point=pt_4, end_point=pt_4, + samples=samples, + start_orientation=i, + end_orientation=j) + simulate_path(path, test_robot) + # Come back + i = test_orn + j = orn_1 + + # Move to point 2 + path = linear_interpolation(start_point=pt_4, end_point=pt_2, + samples=samples, + start_orientation=orn_1, + end_orientation=orn_2) + simulate_path(path, test_robot) - # elementary_operations = create_movement_operations(linear_path, test_robot) - # linear_path.draw(color=[1, 1, 1]) - # run_elementary_operations(elementary_operations) + # Path 1: Linear path + path = linear_interpolation(start_point=pt_2, end_point=pt_1, + samples=samples, + start_orientation=orn_2, + end_orientation=orn_1) + path.draw(color=color_1) + simulate_path(path, test_robot) + + # Path 2: Circular path + path = circular_interpolation(start_point=pt_1, end_point=pt_3, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=orn_1, + end_orientation=orn_1) + path.draw(color=color_2) + simulate_path(path, test_robot) # Path 3: Circular path - circular_path = circular_interpolation(start_point=pt_1, end_point=pt_3, - samples=samples, axis=axis, - clockwise=clockwise, radius=radius, - start_orientation=orn_1, - end_orientation=orn_1) - elementary_operations = create_movement_operations( - circular_path, test_robot) - circular_path.draw(color=color_2) - run_elementary_operations(elementary_operations) - - # Path 4: Circular path clockwise = False - circular_path = circular_interpolation(start_point=pt_3, end_point=pt_2, - samples=samples, axis=axis, - clockwise=clockwise, radius=radius, - start_orientation=orn_1, - end_orientation=orn_1) - elementary_operations = create_movement_operations( - circular_path, test_robot) - circular_path.draw(color=color_3) - run_elementary_operations(elementary_operations) + path = circular_interpolation(start_point=pt_3, end_point=pt_2, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=orn_1, + end_orientation=orn_1) + path.draw(color=color_3) + simulate_path(path, test_robot) # Path 4: Circular path axis = 0 - circular_path = circular_interpolation(start_point=pt_2, end_point=pt_3, - samples=samples, axis=axis, - clockwise=clockwise, radius=radius, - start_orientation=orn_1, - end_orientation=orn_1) - elementary_operations = create_movement_operations( - circular_path, test_robot) - circular_path.draw(color=color_4) - run_elementary_operations(elementary_operations) - - # Path 4: Circular path + path = circular_interpolation(start_point=pt_2, end_point=pt_3, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=orn_1, + end_orientation=orn_1) + path.draw(color=color_4) + simulate_path(path, test_robot) + + # Path 5: Circular path axis = 0 clockwise = True - circular_path = circular_interpolation(start_point=pt_3, end_point=pt_1, - samples=samples, axis=axis, - clockwise=clockwise, radius=radius, - start_orientation=orn_1, - end_orientation=orn_1) - elementary_operations = create_movement_operations( - circular_path, test_robot) - circular_path.draw(color=color_5) - run_elementary_operations(elementary_operations) - - # Path 1: Linear path - linear_path = linear_interpolation(start_point=pt_1, end_point=pt_3, - samples=samples, - start_orientation=orn_1, - end_orientation=orn_1) - elementary_operations = create_movement_operations(linear_path, test_robot) - run_elementary_operations(elementary_operations) - - # Path 4: Circular path + path = circular_interpolation(start_point=pt_3, end_point=pt_1, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=orn_1, + end_orientation=orn_1) + path.draw(color=color_5) + simulate_path(path, test_robot) + + # Path 6: Linear path + path = linear_interpolation(start_point=pt_1, end_point=pt_3, + samples=samples, + start_orientation=orn_1, + end_orientation=orn_1) + simulate_path(path, test_robot) + + # Path 7: Circular path axis = 1 clockwise = True - circular_path = circular_interpolation(start_point=pt_3, end_point=pt_4, - samples=samples, axis=axis, - clockwise=clockwise, radius=radius, - start_orientation=orn_1, - end_orientation=orn_1) - elementary_operations = create_movement_operations( - circular_path, test_robot) - circular_path.draw(color=color_6) - run_elementary_operations(elementary_operations) - - # Path 4: Circular path + path = circular_interpolation(start_point=pt_3, end_point=pt_4, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=orn_1, + end_orientation=orn_1) + path.draw(color=color_6) + simulate_path(path, test_robot) + + # Path 8: Circular path axis = 1 clockwise = False - circular_path = circular_interpolation(start_point=pt_4, end_point=pt_5, - samples=samples, axis=axis, - clockwise=clockwise, radius=radius, - start_orientation=orn_1, - end_orientation=orn_1) - elementary_operations = create_movement_operations( - circular_path, test_robot) - circular_path.draw(color=color_7) - run_elementary_operations(elementary_operations) - - # Path 1: Linear path - linear_path = linear_interpolation(start_point=pt_5, end_point=pt_1, - samples=samples, - start_orientation=orn_1, - end_orientation=orn_1) - elementary_operations = create_movement_operations(linear_path, test_robot) - run_elementary_operations(elementary_operations) + path = circular_interpolation(start_point=pt_4, end_point=pt_5, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=orn_1, + end_orientation=orn_1) + path.draw(color=color_7) + simulate_path(path, test_robot) + + # Path 9: Linear path + path = linear_interpolation(start_point=pt_5, end_point=pt_1, + samples=samples, + start_orientation=orn_1, + end_orientation=orn_1) + simulate_path(path, test_robot) # Path 1: Spline path - spline_path = spline_interpolation( - [pt_1, pt_4, pt_2], samples=samples) - spline_path.draw(color=color_8, pose=True) - spline_path.orientations = np.transpose([orn_1] - * len(spline_path.orientations[0])) - elementary_operations = create_movement_operations( - spline_path, test_robot) - run_elementary_operations(elementary_operations) + path = spline_interpolation( + [pt_1, pt_4, pt_2], samples=samples, start_orientation=orn_1, end_orientation=orn_1) + path.draw(color=color_8, pose=True) + simulate_path(path, test_robot) From f974a2b3d6c139970eeab2d7ea1cf323cfac8a35 Mon Sep 17 00:00:00 2001 From: Philipp Date: Fri, 4 Aug 2023 00:52:51 +0200 Subject: [PATCH 6/9] new update - new update interpolation demo --- examples/interpolation.py | 6 ++ examples/interpolation_dem.py | 143 +++++++++++++--------------------- 2 files changed, 58 insertions(+), 91 deletions(-) diff --git a/examples/interpolation.py b/examples/interpolation.py index f923843..06f02c4 100644 --- a/examples/interpolation.py +++ b/examples/interpolation.py @@ -117,6 +117,9 @@ def circular_interpolation(start_point: np.array, end_point: np.array, 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. + 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 @@ -154,6 +157,9 @@ def spline_interpolation(points: np.array, samples: int, start_orientation: np.a points (np.array(3,n)): A 3 dimensional array whith each dimension containing subsequent positions. samples (int): The number of samples used to interpolate + 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 diff --git a/examples/interpolation_dem.py b/examples/interpolation_dem.py index ac2bc82..750a551 100644 --- a/examples/interpolation_dem.py +++ b/examples/interpolation_dem.py @@ -9,6 +9,38 @@ from pybullet_industrial import ToolPath +def simulate_circular_path(robot: pi.RobotBase, end_point, + end_orientation, + color=None, samples=100, axis=2, + clockwise=True, radius=0.6): + + start_point, start_orientation = robot.get_endeffector_pose() + path = circular_interpolation(start_point=start_point, end_point=end_point, + samples=samples, axis=axis, + clockwise=clockwise, radius=radius, + start_orientation=start_orientation, + end_orientation=end_orientation) + if color is not None: + path.draw(color=color) + + simulate_path(path, robot) + + +def simulate_linear_path(robot: pi.RobotBase, end_point, + end_orientation, + color=None, samples=100): + + start_point, start_orientation = robot.get_endeffector_pose() + path = linear_interpolation(start_point=start_point, end_point=end_point, + samples=samples, + start_orientation=start_orientation, + end_orientation=end_orientation) + if color is not None: + path.draw(color=color) + + simulate_path(path, robot) + + def simulate_path(path: ToolPath, robot: pi.RobotBase): elementary_operations = create_movement_operations(path, robot) run_elementary_operations(elementary_operations) @@ -77,7 +109,7 @@ def create_movement_operations(path: ToolPath, robot: pi.RobotBase): orn_1 = p.getQuaternionFromEuler([-np.pi/2, 0, 0]) orn_2 = p.getQuaternionFromEuler([-3*np.pi/2, 0, 0]) orn_3 = p.getQuaternionFromEuler([0, np.pi/2, 0]) - orn_4 = p.getQuaternionFromEuler([0, 0, np.pi/2]) + orn_4 = p.getQuaternionFromEuler([0, 0, -np.pi/2]) test_orientations = [orn_2, orn_3, orn_4] # Setting the colors @@ -91,124 +123,53 @@ def create_movement_operations(path: ToolPath, robot: pi.RobotBase): color_7 = [1, 1, 1] color_8 = [0.5, 0.5, 0.5] - # Seting the interpolation parameters - samples = 100 - clockwise = True # Circular Direction - radius = 0.6 - axis = 2 - # Setting position - linear_path = linear_interpolation(start_point=pt_start, end_point=pt_4, - samples=samples, - start_orientation=orn_start, - end_orientation=orn_1) - elementary_operations = create_movement_operations(linear_path, test_robot) - run_elementary_operations(elementary_operations) + simulate_linear_path(test_robot, pt_4, orn_1) # Test different orientations for test_orn in test_orientations: i = orn_1 - j = test_orn for _ in range(2): - path = linear_interpolation(start_point=pt_4, end_point=pt_4, - samples=samples, - start_orientation=i, - end_orientation=j) - simulate_path(path, test_robot) + simulate_linear_path( + robot=test_robot, end_point=pt_4, end_orientation=i) # Come back i = test_orn - j = orn_1 # Move to point 2 - path = linear_interpolation(start_point=pt_4, end_point=pt_2, - samples=samples, - start_orientation=orn_1, - end_orientation=orn_2) - simulate_path(path, test_robot) + simulate_linear_path( + robot=test_robot, end_point=pt_2, end_orientation=orn_2) # Path 1: Linear path - path = linear_interpolation(start_point=pt_2, end_point=pt_1, - samples=samples, - start_orientation=orn_2, - end_orientation=orn_1) - path.draw(color=color_1) - simulate_path(path, test_robot) + simulate_linear_path(robot=test_robot, end_point=pt_1, + end_orientation=orn_1, color=color_1) # Path 2: Circular path - path = circular_interpolation(start_point=pt_1, end_point=pt_3, - samples=samples, axis=axis, - clockwise=clockwise, radius=radius, - start_orientation=orn_1, - end_orientation=orn_1) - path.draw(color=color_2) - simulate_path(path, test_robot) + simulate_circular_path(test_robot, pt_3, orn_1, color=color_2) # Path 3: Circular path - clockwise = False - path = circular_interpolation(start_point=pt_3, end_point=pt_2, - samples=samples, axis=axis, - clockwise=clockwise, radius=radius, - start_orientation=orn_1, - end_orientation=orn_1) - path.draw(color=color_3) - simulate_path(path, test_robot) + simulate_circular_path(test_robot, pt_2, orn_1, + color=color_3, clockwise=False) # Path 4: Circular path - axis = 0 - path = circular_interpolation(start_point=pt_2, end_point=pt_3, - samples=samples, axis=axis, - clockwise=clockwise, radius=radius, - start_orientation=orn_1, - end_orientation=orn_1) - path.draw(color=color_4) - simulate_path(path, test_robot) + simulate_circular_path(test_robot, pt_3, orn_1, color=color_4, axis=0) # Path 5: Circular path - axis = 0 - clockwise = True - path = circular_interpolation(start_point=pt_3, end_point=pt_1, - samples=samples, axis=axis, - clockwise=clockwise, radius=radius, - start_orientation=orn_1, - end_orientation=orn_1) - path.draw(color=color_5) - simulate_path(path, test_robot) + simulate_circular_path(test_robot, pt_1, orn_1, + color=color_5, clockwise=False, axis=0) # Path 6: Linear path - path = linear_interpolation(start_point=pt_1, end_point=pt_3, - samples=samples, - start_orientation=orn_1, - end_orientation=orn_1) - simulate_path(path, test_robot) + simulate_linear_path(test_robot, pt_3, orn_1) # Path 7: Circular path - axis = 1 - clockwise = True - path = circular_interpolation(start_point=pt_3, end_point=pt_4, - samples=samples, axis=axis, - clockwise=clockwise, radius=radius, - start_orientation=orn_1, - end_orientation=orn_1) - path.draw(color=color_6) - simulate_path(path, test_robot) + simulate_circular_path(test_robot, pt_4, orn_1, + color=color_6, axis=1) # Path 8: Circular path - axis = 1 - clockwise = False - path = circular_interpolation(start_point=pt_4, end_point=pt_5, - samples=samples, axis=axis, - clockwise=clockwise, radius=radius, - start_orientation=orn_1, - end_orientation=orn_1) - path.draw(color=color_7) - simulate_path(path, test_robot) + simulate_circular_path(test_robot, pt_5, orn_1, + color=color_7, axis=1, clockwise=False) # Path 9: Linear path - path = linear_interpolation(start_point=pt_5, end_point=pt_1, - samples=samples, - start_orientation=orn_1, - end_orientation=orn_1) - simulate_path(path, test_robot) + simulate_linear_path(test_robot, pt_1, orn_1) # Path 1: Spline path From f62b4d39a396ff78082c3516c75b4adbd179785b Mon Sep 17 00:00:00 2001 From: Philipp Date: Sat, 19 Aug 2023 11:44:32 +0200 Subject: [PATCH 7/9] Upddate Interpolation Dem - new implementation of the interpolation demonstration - point are converted into euler before the orientations are interpolated --- examples/interpolation.py | 21 +++++++++++++++++-- examples/interpolation_dem.py | 38 +++++++++++++++-------------------- 2 files changed, 35 insertions(+), 24 deletions(-) diff --git a/examples/interpolation.py b/examples/interpolation.py index 06f02c4..e9dfadc 100644 --- a/examples/interpolation.py +++ b/examples/interpolation.py @@ -1,6 +1,7 @@ import numpy as np import scipy.interpolate as sci from scipy.spatial.transform import Rotation +import pybullet as p from pybullet_industrial.toolpath import ToolPath @@ -54,9 +55,17 @@ def linear_interpolation(start_point: np.array, end_point: np.array, 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_path.orientations = orientations.transpose() + final_orientations = [] + + for orientation in orientations: + orientation_quat = p.getQuaternionFromEuler(orientation) + final_orientations.append(orientation_quat) + + final_path.orientations = np.array(final_orientations).transpose() return final_path @@ -142,9 +151,17 @@ def circular_interpolation(start_point: np.array, end_point: np.array, 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_path.orientations = orientations.transpose() + final_orientations = [] + + for orientation in orientations: + orientation_quat = p.getQuaternionFromEuler(orientation) + final_orientations.append(orientation_quat) + + final_path.orientations = np.array(final_orientations).transpose() return final_path diff --git a/examples/interpolation_dem.py b/examples/interpolation_dem.py index 750a551..a765c07 100644 --- a/examples/interpolation_dem.py +++ b/examples/interpolation_dem.py @@ -5,7 +5,6 @@ import pybullet_industrial as pi from interpolation import linear_interpolation from interpolation import circular_interpolation -from interpolation import spline_interpolation from pybullet_industrial import ToolPath @@ -99,18 +98,20 @@ def create_movement_operations(path: ToolPath, robot: pi.RobotBase): # Setting the points pt_start, orn_start = test_robot.get_endeffector_pose() - pt_1 = [1.5, -2, 2] - pt_2 = [1.5, 0, 2] - pt_3 = [1.5, -1, 2] - pt_4 = [2, -1, 2] - pt_5 = [2.5, -1, 2] + pt_1 = [1.8, -2, 2] + pt_2 = [1.8, 0, 2] + pt_3 = [1.8, -1, 2] + pt_4 = [2.1, -1, 2] + pt_5 = [2.4, -1, 2] # Setting the orientations orn_1 = p.getQuaternionFromEuler([-np.pi/2, 0, 0]) - orn_2 = p.getQuaternionFromEuler([-3*np.pi/2, 0, 0]) - orn_3 = p.getQuaternionFromEuler([0, np.pi/2, 0]) - orn_4 = p.getQuaternionFromEuler([0, 0, -np.pi/2]) - test_orientations = [orn_2, orn_3, orn_4] + orn_2 = p.getQuaternionFromEuler([-np.pi/2, -np.pi/2, 0]) + orn_3 = p.getQuaternionFromEuler([-np.pi/2, -np.pi/2, -np.pi/2]) + + orn_4 = p.getQuaternionFromEuler([-np.pi/4, 0, -np.pi/2]) + + test_orientations = [orn_2, orn_3] # Setting the colors @@ -124,14 +125,14 @@ def create_movement_operations(path: ToolPath, robot: pi.RobotBase): color_8 = [0.5, 0.5, 0.5] # Setting position - simulate_linear_path(test_robot, pt_4, orn_1) + simulate_linear_path(test_robot, pt_5, orn_1) # Test different orientations for test_orn in test_orientations: i = orn_1 for _ in range(2): simulate_linear_path( - robot=test_robot, end_point=pt_4, end_orientation=i) + robot=test_robot, end_point=pt_5, end_orientation=i) # Come back i = test_orn @@ -147,14 +148,14 @@ def create_movement_operations(path: ToolPath, robot: pi.RobotBase): simulate_circular_path(test_robot, pt_3, orn_1, color=color_2) # Path 3: Circular path - simulate_circular_path(test_robot, pt_2, orn_1, + simulate_circular_path(test_robot, pt_2, orn_2, color=color_3, clockwise=False) # Path 4: Circular path simulate_circular_path(test_robot, pt_3, orn_1, color=color_4, axis=0) # Path 5: Circular path - simulate_circular_path(test_robot, pt_1, orn_1, + simulate_circular_path(test_robot, pt_1, orn_2, color=color_5, clockwise=False, axis=0) # Path 6: Linear path @@ -169,11 +170,4 @@ def create_movement_operations(path: ToolPath, robot: pi.RobotBase): color=color_7, axis=1, clockwise=False) # Path 9: Linear path - simulate_linear_path(test_robot, pt_1, orn_1) - - # Path 1: Spline path - - path = spline_interpolation( - [pt_1, pt_4, pt_2], samples=samples, start_orientation=orn_1, end_orientation=orn_1) - path.draw(color=color_8, pose=True) - simulate_path(path, test_robot) + simulate_linear_path(test_robot, pt_1, orn_2) From 2edee469c0fefcf4390e7d80df203172f8351e04 Mon Sep 17 00:00:00 2001 From: Philipp Date: Sun, 20 Aug 2023 09:43:03 +0200 Subject: [PATCH 8/9] Final adjustment of the interpolation demo Adjustments Interpolation Class: - Linear-, circular-, spline interpolation adapted to run interpolated orientations interpolation_dem: - demonstration of all the possible interpolations --- examples/interpolation.py | 203 ----------------------- examples/interpolation_dem.py | 70 ++++---- src/pybullet_industrial/interpolation.py | 99 ++++++++--- 3 files changed, 111 insertions(+), 261 deletions(-) delete mode 100644 examples/interpolation.py diff --git a/examples/interpolation.py b/examples/interpolation.py deleted file mode 100644 index e9dfadc..0000000 --- a/examples/interpolation.py +++ /dev/null @@ -1,203 +0,0 @@ -import numpy as np -import scipy.interpolate as sci -from scipy.spatial.transform import Rotation -import pybullet as p - -from pybullet_industrial.toolpath import ToolPath - - -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 - - Args: - center (np.array): the center of the circle - 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 - clockwise (bool): boolean value indicating if the interpolation is performed clockwise - or anticlockwise - - Returns: - np.array: array of 2 dimensional path points - """ - - 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 - else: - path_angle = min_angle+j*(max_angle-min_angle)/step_num - new_position = center + radius * \ - np.array([np.cos(path_angle), np.sin(path_angle)]) - circular_path[:, j] = new_position - return circular_path - - -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 - - 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 - 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 - """ - 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 = [] - - for orientation in orientations: - orientation_quat = p.getQuaternionFromEuler(orientation) - final_orientations.append(orientation_quat) - - final_path.orientations = np.array(final_orientations).transpose() - - return final_path - - -def planar_circular_interpolation(start_point: np.array, end_point: np.array, - radius: float, samples: int, clockwise: bool = True): - """Helper function which performs a circular interpolation in the x-y plane - - 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 - samples (int): The number of samples used to interpolate - clockwise (bool): boolean value indicating if the interpolation is performed clockwise - or anticlockwise - - Returns: - np.array: The interpolated path - """ - connecting_line = end_point-start_point - distance_between_points = np.linalg.norm(connecting_line) - if radius <= distance_between_points/2: - raise ValueError("The radius needs to be at least " + - str(distance_between_points/2)) - - center_distance_from_connecting_line = np.sqrt( - radius**2-distance_between_points**2/4) - - if clockwise: - orthogonal_vector = np.array( - [connecting_line[1], -1*connecting_line[0]]) - else: - orthogonal_vector = np.array( - [-1*connecting_line[1], connecting_line[0]]) - - circle_center = start_point+connecting_line/2+center_distance_from_connecting_line * \ - orthogonal_vector/np.linalg.norm(orthogonal_vector) - - angle_range = np.arccos(center_distance_from_connecting_line/radius)*2 - initial_angle = np.arctan2( - start_point[1]-circle_center[1], start_point[0]-circle_center[0]) - - planar_path = build_circular_path( - circle_center, radius, initial_angle, initial_angle+angle_range, samples, clockwise) - return planar_path - - -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 - - 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 - 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. - 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) - planar_start_point = np.array( - [start_point[all_axis[0]], start_point[all_axis[1]]]) - planar_end_point = np.array( - [end_point[all_axis[0]], end_point[all_axis[1]]]) - - planar_path = planar_circular_interpolation( - planar_start_point, planar_end_point, radius, samples, clockwise) - - positions = np.zeros((3, samples)) - 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 = [] - - for orientation in orientations: - orientation_quat = p.getQuaternionFromEuler(orientation) - final_orientations.append(orientation_quat) - - final_path.orientations = np.array(final_orientations).transpose() - - return final_path - - -def spline_interpolation(points: np.array, samples: int, start_orientation: np.array = None, - end_orientation: np.array = None): - """Interpolates between a number of points in cartesian space. - - Args: - points (np.array(3,n)): A 3 dimensional array whith each dimension containing - subsequent positions. - samples (int): The number of samples used to interpolate - 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 - """ - s = np.linspace(0, 1, len(points[0])) - - positions = np.zeros((3, samples)) - cs_x = sci.CubicSpline(s, points[0]) - cs_y = sci.CubicSpline(s, points[1]) - cs_z = sci.CubicSpline(s, points[2]) - - cs_s = np.linspace(0, 1, samples) - positions[0] = cs_x(cs_s) - positions[1] = cs_y(cs_s) - positions[2] = cs_z(cs_s) - - final_path = ToolPath(positions=positions) - - if start_orientation is not None and end_orientation is not None: - orientations = np.linspace(start_orientation, - end_orientation, num=samples) - final_path.orientations = orientations.transpose() - - return final_path diff --git a/examples/interpolation_dem.py b/examples/interpolation_dem.py index a765c07..e145ac6 100644 --- a/examples/interpolation_dem.py +++ b/examples/interpolation_dem.py @@ -3,16 +3,27 @@ import numpy as np import pybullet_data import pybullet_industrial as pi -from interpolation import linear_interpolation -from interpolation import circular_interpolation +from pybullet_industrial import linear_interpolation +from pybullet_industrial import circular_interpolation from pybullet_industrial import ToolPath -def simulate_circular_path(robot: pi.RobotBase, end_point, - end_orientation, - color=None, samples=100, axis=2, +def simulate_circular_path(robot: pi.RobotBase, end_point: np.array, + end_orientation: np.array, + color=None, samples=200, axis=2, clockwise=True, radius=0.6): + """Creates a linear path and calls the simulate function + Args: + robot (RobotBase): operating robot + end_point(np.array): next point + end_orientation(np.array): next orientation + color: color for drawing the path + samples: number of samples used to interpolate + axis: The axis around which the circle is interpolated + clockwise: direction of the interpolation + radius: radius of the interpolation + """ start_point, start_orientation = robot.get_endeffector_pose() path = circular_interpolation(start_point=start_point, end_point=end_point, samples=samples, axis=axis, @@ -21,14 +32,21 @@ def simulate_circular_path(robot: pi.RobotBase, end_point, end_orientation=end_orientation) if color is not None: path.draw(color=color) - simulate_path(path, robot) -def simulate_linear_path(robot: pi.RobotBase, end_point, - end_orientation, - color=None, samples=100): +def simulate_linear_path(robot: pi.RobotBase, end_point: np.array, + end_orientation: np.array, + color=None, samples=200): + """Creates a linear path and calls the simulate function + Args: + robot (RobotBase): operating robot + end_point(np.array): next point + end_orientation(np.array): next orientation + color: color for drawing the path + samples: number of samples used to interpolate + """ start_point, start_orientation = robot.get_endeffector_pose() path = linear_interpolation(start_point=start_point, end_point=end_point, samples=samples, @@ -36,19 +54,21 @@ def simulate_linear_path(robot: pi.RobotBase, end_point, end_orientation=end_orientation) if color is not None: path.draw(color=color) - simulate_path(path, robot) def simulate_path(path: ToolPath, robot: pi.RobotBase): - elementary_operations = create_movement_operations(path, robot) - run_elementary_operations(elementary_operations) + """Simulates the created path by running the elementary operations + Args: + path (ToolPath): input tool path + robot (RobotBase): operating robot + """ + elementary_operations = create_movement_operations(path, robot) -def run_elementary_operations(elementary_operations: list): for operation in elementary_operations: operation() - for _ in range(200): + for _ in range(100): p.stepSimulation() @@ -57,7 +77,8 @@ def create_movement_operations(path: ToolPath, robot: pi.RobotBase): on a given tool path and active endeffector. Args: - path(ToolPath): input tool path + path (ToolPath): input tool path + robot (RobotBase): operating Robot Returns: elementary_operations(list): elementary operations to move robot @@ -108,13 +129,9 @@ def create_movement_operations(path: ToolPath, robot: pi.RobotBase): orn_1 = p.getQuaternionFromEuler([-np.pi/2, 0, 0]) orn_2 = p.getQuaternionFromEuler([-np.pi/2, -np.pi/2, 0]) orn_3 = p.getQuaternionFromEuler([-np.pi/2, -np.pi/2, -np.pi/2]) - - orn_4 = p.getQuaternionFromEuler([-np.pi/4, 0, -np.pi/2]) - - test_orientations = [orn_2, orn_3] + orn_4 = p.getQuaternionFromEuler([-np.pi, 0, 0]) # Setting the colors - color_1 = [1, 0, 0] color_2 = [0, 1, 0] color_3 = [0, 0, 1] @@ -127,15 +144,6 @@ def create_movement_operations(path: ToolPath, robot: pi.RobotBase): # Setting position simulate_linear_path(test_robot, pt_5, orn_1) - # Test different orientations - for test_orn in test_orientations: - i = orn_1 - for _ in range(2): - simulate_linear_path( - robot=test_robot, end_point=pt_5, end_orientation=i) - # Come back - i = test_orn - # Move to point 2 simulate_linear_path( robot=test_robot, end_point=pt_2, end_orientation=orn_2) @@ -148,7 +156,7 @@ def create_movement_operations(path: ToolPath, robot: pi.RobotBase): simulate_circular_path(test_robot, pt_3, orn_1, color=color_2) # Path 3: Circular path - simulate_circular_path(test_robot, pt_2, orn_2, + simulate_circular_path(test_robot, pt_2, orn_4, color=color_3, clockwise=False) # Path 4: Circular path @@ -162,7 +170,7 @@ def create_movement_operations(path: ToolPath, robot: pi.RobotBase): simulate_linear_path(test_robot, pt_3, orn_1) # Path 7: Circular path - simulate_circular_path(test_robot, pt_4, orn_1, + simulate_circular_path(test_robot, pt_4, orn_2, color=color_6, axis=1) # Path 8: Circular path diff --git a/src/pybullet_industrial/interpolation.py b/src/pybullet_industrial/interpolation.py index f10980d..c6cc926 100644 --- a/src/pybullet_industrial/interpolation.py +++ b/src/pybullet_industrial/interpolation.py @@ -1,12 +1,13 @@ import numpy as np import scipy.interpolate as sci -from scipy.spatial.transform import Rotation +import pybullet as p from pybullet_industrial.toolpath import ToolPath def build_circular_path(center: np.array, radius: float, - min_angle: float, max_angle: float, step_num: int, clockwise: bool = True): + min_angle: float, max_angle: float, step_num: int, + clockwise: bool = True): """Function which builds a circular path Args: @@ -15,8 +16,8 @@ def build_circular_path(center: np.array, radius: float, 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 - clockwise (bool): boolean value indicating if the interpolation is performed clockwise - or anticlockwise + clockwise (bool): boolean value indicating if the interpolation is + performed clockwise or anticlockwise Returns: np.array: array of 2 dimensional path points @@ -36,8 +37,7 @@ 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, - ): + end_orientation: np.array = None): """Performs a linear interpolation betwenn two points in 3D space Args: @@ -50,20 +50,20 @@ def linear_interpolation(start_point: np.array, end_point: np.array, Returns: ToolPath: A ToolPath object of the interpolated path """ - final_path = np.linspace(start_point, end_point, num=samples) + positions = np.linspace(start_point, end_point, num=samples) - start_rotation = Rotation.from_quat(start_orientation) - end_rotation = Rotation.from_quat(end_orientation) + final_path = ToolPath(positions=positions.transpose()) - # Perform quaternion interpolation - interpolated_rotations = start_rotation.interpolate( - end_rotation, samples) + if start_orientation is not None and end_orientation is not None: + final_path.orientations = orientation_interpolation( + start_orientation, end_orientation, samples=samples) - return ToolPath(final_path.transpose(), final_orientations.transpose()) + return final_path def planar_circular_interpolation(start_point: np.array, end_point: np.array, - radius: float, samples: int, clockwise: bool = True): + radius: float, samples: int, + clockwise: bool = True): """Helper function which performs a circular interpolation in the x-y plane Args: @@ -71,8 +71,8 @@ def planar_circular_interpolation(start_point: np.array, end_point: np.array, 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 - clockwise (bool): boolean value indicating if the interpolation is performed clockwise - or anticlockwise + clockwise (bool): boolean value indicating if the interpolation is + performed clockwise or anticlockwise Returns: np.array: The interpolated path @@ -106,7 +106,10 @@ def planar_circular_interpolation(start_point: np.array, end_point: np.array, def circular_interpolation(start_point: np.array, end_point: np.array, - radius: float, samples: int, axis: int = 2, clockwise: bool = True): + 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 Args: @@ -117,6 +120,9 @@ def circular_interpolation(start_point: np.array, end_point: np.array, 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. + 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 @@ -132,34 +138,73 @@ def circular_interpolation(start_point: np.array, end_point: np.array, planar_path = planar_circular_interpolation( planar_start_point, planar_end_point, radius, samples, clockwise) - path = np.zeros((3, samples)) + positions = np.zeros((3, samples)) for i in range(2): - path[all_axis[i]] = planar_path[i] - path[axis] = np.linspace(start_point[axis], end_point[axis], samples) - return ToolPath(path) + 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: + final_path.orientations = orientation_interpolation( + start_orientation, end_orientation, samples=samples) + + return final_path + + +def orientation_interpolation(start_orientation: np.array, + end_orientation: np.array, samples: int): + """Interpolates between two orientations + + Args: + start_orientation (np.array(3,1)): 3-D array containing the start orientation + end_orientation (np.array(3,1)): 3-D array containing the end orientation + samples (int): The number of samples used to interpolate + """ + start_orientation = p.getEulerFromQuaternion(start_orientation) + end_orientation = p.getEulerFromQuaternion(end_orientation) + euler_orientations = np.linspace(start_orientation, + end_orientation, num=samples) + orientations = [] + + for euler_orientation in euler_orientations: + orientation_quat = p.getQuaternionFromEuler(euler_orientation) + orientations.append(orientation_quat) + return np.array(orientations).transpose() -def spline_interpolation(points: np.array, samples: int): + +def spline_interpolation(points: np.array, samples: int, start_orientation: np.array = None, + end_orientation: np.array = None): """Interpolates between a number of points in cartesian space. Args: points (np.array(3,n)): A 3 dimensional array whith each dimension containing subsequent positions. samples (int): The number of samples used to interpolate + 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 """ s = np.linspace(0, 1, len(points[0])) - path = np.zeros((3, samples)) + positions = np.zeros((3, samples)) cs_x = sci.CubicSpline(s, points[0]) cs_y = sci.CubicSpline(s, points[1]) cs_z = sci.CubicSpline(s, points[2]) cs_s = np.linspace(0, 1, samples) - path[0] = cs_x(cs_s) - path[1] = cs_y(cs_s) - path[2] = cs_z(cs_s) + positions[0] = cs_x(cs_s) + positions[1] = cs_y(cs_s) + positions[2] = cs_z(cs_s) + + final_path = ToolPath(positions=positions) + + if start_orientation is not None and end_orientation is not None: + orientations = np.linspace(start_orientation, + end_orientation, num=samples) + final_path.orientations = orientations.transpose() - return ToolPath(path) + return final_path From 9e09485764a87b358e3dd28a569007366aed296a Mon Sep 17 00:00:00 2001 From: Philipp Date: Wed, 27 Sep 2023 14:50:28 +0200 Subject: [PATCH 9/9] interpolation Update - The spline Interopolation was reset to the default method - Interpolating through orientations works by converting from quaternion to euler and back --- src/pybullet_industrial/interpolation.py | 92 ++++++++++-------------- 1 file changed, 36 insertions(+), 56 deletions(-) diff --git a/src/pybullet_industrial/interpolation.py b/src/pybullet_industrial/interpolation.py index c6cc926..6f2c93e 100644 --- a/src/pybullet_industrial/interpolation.py +++ b/src/pybullet_industrial/interpolation.py @@ -6,8 +6,7 @@ def build_circular_path(center: np.array, radius: float, - min_angle: float, max_angle: float, step_num: int, - clockwise: bool = True): + min_angle: float, max_angle: float, step_num: int, clockwise: bool = True): """Function which builds a circular path Args: @@ -16,8 +15,8 @@ def build_circular_path(center: np.array, radius: float, 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 - clockwise (bool): boolean value indicating if the interpolation is - performed clockwise or anticlockwise + clockwise (bool): boolean value indicating if the interpolation is performed clockwise + or anticlockwise Returns: np.array: array of 2 dimensional path points @@ -35,8 +34,8 @@ def build_circular_path(center: np.array, radius: float, return circular_path -def linear_interpolation(start_point: np.array, end_point: np.array, - samples: int, start_orientation: np.array = None, +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 @@ -51,19 +50,26 @@ def linear_interpolation(start_point: np.array, end_point: np.array, ToolPath: A ToolPath object of the interpolated path """ 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: - final_path.orientations = orientation_interpolation( - start_orientation, end_orientation, samples=samples) + start_orientation = p.getEulerFromQuaternion(start_orientation) + end_orientation = p.getEulerFromQuaternion(end_orientation) + orientations = np.linspace(start_orientation, + end_orientation, num=samples) + final_orientations = [] + + for orientation in orientations: + orientation_quat = p.getQuaternionFromEuler(orientation) + final_orientations.append(orientation_quat) + + final_path.orientations = np.array(final_orientations).transpose() return final_path def planar_circular_interpolation(start_point: np.array, end_point: np.array, - radius: float, samples: int, - clockwise: bool = True): + radius: float, samples: int, clockwise: bool = True): """Helper function which performs a circular interpolation in the x-y plane Args: @@ -71,8 +77,8 @@ def planar_circular_interpolation(start_point: np.array, end_point: np.array, 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 - clockwise (bool): boolean value indicating if the interpolation is - performed clockwise or anticlockwise + clockwise (bool): boolean value indicating if the interpolation is performed clockwise + or anticlockwise Returns: np.array: The interpolated path @@ -107,8 +113,7 @@ def planar_circular_interpolation(start_point: np.array, end_point: np.array, 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, + clockwise: bool = True, start_orientation: np.array = None, end_orientation: np.array = None): """AI is creating summary for circular_interpolation @@ -123,7 +128,6 @@ def circular_interpolation(start_point: np.array, end_point: np.array, 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 """ @@ -145,66 +149,42 @@ def circular_interpolation(start_point: np.array, end_point: np.array, final_path = ToolPath(positions=positions) if start_orientation is not None and end_orientation is not None: - final_path.orientations = orientation_interpolation( - start_orientation, end_orientation, samples=samples) - - return final_path - - -def orientation_interpolation(start_orientation: np.array, - end_orientation: np.array, samples: int): - """Interpolates between two orientations + start_orientation = p.getEulerFromQuaternion(start_orientation) + end_orientation = p.getEulerFromQuaternion(end_orientation) + orientations = np.linspace(start_orientation, + end_orientation, num=samples) + final_orientations = [] - Args: - start_orientation (np.array(3,1)): 3-D array containing the start orientation - end_orientation (np.array(3,1)): 3-D array containing the end orientation - samples (int): The number of samples used to interpolate - """ - start_orientation = p.getEulerFromQuaternion(start_orientation) - end_orientation = p.getEulerFromQuaternion(end_orientation) - euler_orientations = np.linspace(start_orientation, - end_orientation, num=samples) - orientations = [] + for orientation in orientations: + orientation_quat = p.getQuaternionFromEuler(orientation) + final_orientations.append(orientation_quat) - for euler_orientation in euler_orientations: - orientation_quat = p.getQuaternionFromEuler(euler_orientation) - orientations.append(orientation_quat) + final_path.orientations = np.array(final_orientations).transpose() - return np.array(orientations).transpose() + return final_path -def spline_interpolation(points: np.array, samples: int, start_orientation: np.array = None, - end_orientation: np.array = None): +def spline_interpolation(points: np.array, samples: int): """Interpolates between a number of points in cartesian space. Args: points (np.array(3,n)): A 3 dimensional array whith each dimension containing subsequent positions. samples (int): The number of samples used to interpolate - 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 """ s = np.linspace(0, 1, len(points[0])) - positions = np.zeros((3, samples)) + path = np.zeros((3, samples)) cs_x = sci.CubicSpline(s, points[0]) cs_y = sci.CubicSpline(s, points[1]) cs_z = sci.CubicSpline(s, points[2]) cs_s = np.linspace(0, 1, samples) - positions[0] = cs_x(cs_s) - positions[1] = cs_y(cs_s) - positions[2] = cs_z(cs_s) + path[0] = cs_x(cs_s) + path[1] = cs_y(cs_s) + path[2] = cs_z(cs_s) - final_path = ToolPath(positions=positions) - - if start_orientation is not None and end_orientation is not None: - orientations = np.linspace(start_orientation, - end_orientation, num=samples) - final_path.orientations = orientations.transpose() - - return final_path + return ToolPath(path)