Как реализовать функцию 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,:])

Однако, как мне реализовать эту функцию в функции trajectoryMover, показанной ниже, так, чтобы для каждой промежуточной позиции рычага, ориентация руки одновременно интерполируется с использованием метода 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)

Заранее спасибо.