What method can be used to interpolate the orientation of a robotic arm?

Question

I am currently writing a program that when a 6 DOF robotic arm is provided with an initial pose (which is comprised of a position and orientation), a series of intermediate poses and a target pose, a trajectory is calculated that passes through these points. I have successfully implemented a cubic spline interpolation algorithm that calculates a trajectory for the positions of the robotic arm, however I am unsure how to interpolate the orientation of the arm. The orientation matrix has four variables: x, y, z and w. From what I understand from the research I have conducted, quaternions appear to be involved? However I am unsure of what approach/method I ought to implement?

I've written the following function that implements the SLERP method, as advised:

def slerp(self, start_O, target_O, t_array):
        t_array = np.array(t_array)
        start_O = np.array(start_O)
        target_O = np.array(target_O)
        dot = np.sum(start_O*target_O)

        if (dot < 0.0):
            target_O = -target_O
            dot = -dot

        DOT_THRESHOLD = 0.9995
        if (dot > DOT_THRESHOLD):
            result = target_O[np.newaxis,:] + t_array[:,np.newaxis]*(target_O - start_0)[np.newaxis,:]
            return (result.T / np.linalg.norm(result, axis=1)).T

        theta_0 = np.arrcos(dot)
        sin_theta_0 = np.sin(theta_0)
        theta = theat_0 * t_array
        sin_theta = np.sign(theta)

        s0 = np.cos(theta) - dot * sin_theta / sin_theta_0
        s1 = sin_theta / sin_theta_0
        return (s0[:,np.newaxis] * v0[np.newaxis,:]) + (s1[:,np.newaxis] * v1[np.newaxis,:])

However, how do I implement this function in the function trajectoryMover, shown below, such that for each intermediate position of the arm, the orientation of the arm is simultaneously interpolated using the SLERP method? Currently, the x,y,z and w orientations are set as constants, however these ought to become variables corresponding to the current interpolated orientation values, if I understand correctly?

def trajectoryMover(self):
        newPose = Pose()
        arr1 = []
        arr2 = []
        arr3 = []
        x_axis = [0.001, 0.0039, 0.0160, 0.0334]
        y_axis = [0.009, 0.0239, 0.0121, 0.0034]
        z_axis = [0.009, 0.0199, 0.0821, 0.1034]
        start_orientation = [0.707106781172, 0.707106781191, 2.59734823723e-06, 0]
        end_orientation = [0.151231412, 0.5112315134, 0.0051534141, 0.5
        self.cubicSplineInterpolate(x_axis,y_axis,z_axis)
        arr1 = self.xLinespace
        arr2 = self.yLinespace
        arr3 = self.zLinespace

        for x, y, z in zip(arr1, arr2, arr3):

            newPose.position.x = x
            newPose.position.y = y
            newPose.position.z = z
            newPose.orientation.x = 0.707106781172
            newPose.orientation.y = 0.707106781191
            newPose.orientation.z = 2.59734823723e-06
            newPose.orientation.w = 0
            self.set_position_cartesian.publish(newPose)
            rospy.loginfo(newPose)
            rospy.sleep(0.05)

Thanks in advance.

---

Originally posted by 691TReilly on ROS Answers with karma: 1 on 2019-07-03

Post score: 0

Answer 1

Interpolating between two rotation quaternions is a non-trivial bit of mathematics. Luckily this function is built into the TF2 quaternion class in ROS.

The function you want is slightly cryptically called SLERP (Spherical Linear Interpolation). This method is described here, it is a method to interpolate between this quaternion and other given quaternion at any ratio.

This will do all the hard work for you.

---

Originally posted by PeteBlackerThe3rd with karma: 9529 on 2019-07-03

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by 691TReilly on 2019-07-03:
Thank you for your help! I've implemented the SLERP method as you recommended (see amended question above), however how do I implement this function in the function trajectoryMover, shown below, such that for each intermediate position of the arm, the orientation of the arm is simultaneously interpolated using the SLERP method? Currently, the x,y,z and w orientations are set as constants, however these ought to become variables corresponding to the current interpolated orientation values, if I understand correctly?

Comment by gvdhoorn on 2019-07-03:
I don't believe the suggestion was to implement slerp yourself, rather to use the implementation provided by TF2.

But I have the impression your question is not really ROS related, hence the reimplementation.

Can you please edit your original question text to make it clear why this is posted on ROS Answers?