Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Why does SLERP implementation not work correctly?

I am attempting to interpolate the orientation of the end effector of a robot from a starting orientation ([0.707106781172, 0.707106781191, 2.59734823723e-06, -2.59734823723e-06]) to an end orientation ([0.151231412, 0.5112315134, 0.0051534141, 0.015161]). After researching possible approaches, I decided to implement the SLERP algorithm, however, after executing the code, it would appear that the end effector does not reach the end goal. I have tried varying the value of t_array, however the outcome is still the same. After executing the code, the final array (i.e. the incorrect end orientation) is [1.30875068e-01, 5.38345678e-01, 5.83280807e-03, 1.71603152e-02], which, although it is close to the required values, is not accurate. Have I implemented the algorithm correctly? Should I be changing the value of w (which I'm not 100% clear about so if anyone could enlighten me about this variable, it would be greatly appreciated!)?

import numpy as np

def slerp(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,:])

start_orientation = [0.707106781172, 0.707106781191, 2.59734823723e-06, -2.59734823723e-06]
end_orientation = [0.151231412, 0.5112315134, 0.0051534141, 0.015161]
arr_Orient = slerp(start_orientation, end_orientation, np.arange(0,1,0.005))
print(arr_Orient)