Why does SLERP implementation not work correctly? [closed]
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)
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.