如何实现SLERP功能以改变机械臂的方向

Question

我目前正在编写一个程序，当一个6自由度的机械臂具有一个初始姿势（由位置和方向组成）、一系列中间姿势和一个目标姿势时，将计算通过这些点的轨迹。我已经成功地实现了一个三次样条插值算法，该算法可以计算机械臂位置的轨迹，但是我不确定如何插值机械臂的方向。方向矩阵有四个变量：x、y、z和w。如建议的那样，我编写了实现SLERP方法的以下函数：

 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.arccos(dot)
            sin_theta_0 = np.sin(theta_0)
            theta = theta_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] * start_O[np.newaxis,:]) + (s1[:,np.newaxis] * target_O[np.newaxis,:])

但是，我如何在如下所示的功能轨迹移动器中实现此功能，以便对于手臂的每个中间位置，使用SLERP方法同时插值手臂的方向？目前，x、y、z和w方向设置为常量，但是如果我理解正确，这些应该成为与当前插值方向值相对应的变量

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)
        self.slerp(start_orientation, end_orientation, np.arange(0,1,0.001))
        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)

提前谢谢