Adding the Interplolation of the Orientation

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.

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)

src/pybullet_industrial/interpolation.py

@@ -1,11 +1,13 @@
 import numpy as np
 import scipy.interpolate as sci
+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:
@@ -14,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
@@ -33,32 +35,44 @@ 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:
+        final_path.orientations = orientation_interpolation(
+            start_orientation, end_orientation, samples=samples)
+
+    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:
         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
+        clockwise (bool): boolean value indicating if the interpolation is
+                          performed clockwise or anticlockwise
 
     Returns:
         np.array: The interpolated path
@@ -92,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:
@@ -103,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
@@ -118,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 spline_interpolation(points: np.array, samples: int):
+
+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, 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