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 new file mode 100644 index 0000000..e145ac6 --- /dev/null +++ b/examples/interpolation_dem.py @@ -0,0 +1,181 @@ +import os +import pybullet as p +import numpy as np +import pybullet_data +import pybullet_industrial as pi +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: 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, + 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: 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, + 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): + """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) + + for operation in elementary_operations: + operation() + for _ in range(100): + 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 + robot (RobotBase): operating Robot + + 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.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([-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, 0, 0]) + + # 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] + + # Setting position + simulate_linear_path(test_robot, pt_5, orn_1) + + # Move to point 2 + simulate_linear_path( + robot=test_robot, end_point=pt_2, end_orientation=orn_2) + + # Path 1: Linear path + simulate_linear_path(robot=test_robot, end_point=pt_1, + end_orientation=orn_1, color=color_1) + + # Path 2: Circular path + simulate_circular_path(test_robot, pt_3, orn_1, color=color_2) + + # Path 3: Circular path + simulate_circular_path(test_robot, pt_2, orn_4, + 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_2, + color=color_5, clockwise=False, axis=0) + + # Path 6: Linear path + simulate_linear_path(test_robot, pt_3, orn_1) + + # Path 7: Circular path + simulate_circular_path(test_robot, pt_4, orn_2, + color=color_6, axis=1) + + # Path 8: Circular path + simulate_circular_path(test_robot, pt_5, orn_1, + color=color_7, axis=1, clockwise=False) + + # Path 9: Linear path + simulate_linear_path(test_robot, pt_1, orn_2) diff --git a/src/pybullet_industrial/interpolation.py b/src/pybullet_industrial/interpolation.py index 76aa91f..6f2c93e 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 +import pybullet as p from pybullet_industrial.toolpath import ToolPath @@ -33,19 +34,38 @@ 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()) + 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, @@ -92,7 +112,9 @@ 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: @@ -103,6 +125,8 @@ 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 @@ -118,11 +142,26 @@ 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: + 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):