# 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)


edit retag close merge delete

Sort by » oldest newest most voted 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.

more

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?

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?