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.loginfo(newPose)
rospy.sleep(0.05)
Thanks in advance.