Why does SLERP implementation not work correctly? [closed]

asked 2019-07-25 17:46:46 -0500

691TReilly gravatar image

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))
edit retag flag offensive reopen merge delete

Closed for the following reason question is off-topic or not relevant. Please see http://wiki.ros.org/Support for more details. by jayess
close date 2019-07-25 19:43:20.237144


Although this may be robotics-related, it is not ROS-related so I've closed it. If you can update it to be ROS-related then it may be re-opened. With over 45,000 questions on the site we aim to keep it as focused as possible. Perhaps you can try one of the Stack Exchange/Overflow sites.

jayess gravatar image jayess  ( 2019-07-25 19:45:41 -0500 )edit