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

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.sleep(0.05)


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

Post score: 0

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