From d0f94aaafa20dfdad2180f13f16089ad8bb74c31 Mon Sep 17 00:00:00 2001
From: John Wason <wason@wasontech.com>
Date: Tue, 18 Jul 2023 19:17:43 -0400
Subject: [PATCH] Add slerp function (#22)

---
 src/general_robotics_toolbox/__init__.py      |  4 +--
 .../general_robotics_toolbox.py               | 32 +++++++++++++++++++
 test/test_general_robotics_toolbox.py         | 29 +++++++++++++++++
 3 files changed, 63 insertions(+), 2 deletions(-)

diff --git a/src/general_robotics_toolbox/__init__.py b/src/general_robotics_toolbox/__init__.py
index d72882a..c39d0f9 100644
--- a/src/general_robotics_toolbox/__init__.py
+++ b/src/general_robotics_toolbox/__init__.py
@@ -3,7 +3,7 @@
 from .general_robotics_toolbox import hat, invhat, rot, R2rot, screw_matrix, q2R, R2q, q2rot, rot2q, quatcomplement, \
     quatproduct, quatjacobian, rpy2R, R2rpy, Robot, Transform, fwdkin, robotjacobian, subproblem0, subproblem1, \
     subproblem2, subproblem3, subproblem4, apply_robot_aux_transforms, unapply_robot_aux_transforms, \
-    identity_transform, random_R, random_p, random_transform
+    identity_transform, random_R, random_p, random_transform, slerp
 
 from .general_robotics_toolbox_invkin import robot6_sphericalwrist_invkin, ur_invkin, equivalent_configurations, \
     iterative_invkin
@@ -12,5 +12,5 @@
     'quatproduct', 'quatjacobian', 'rpy2R', 'R2rpy', 'Robot', 'Transform', 'fwdkin', 'robotjacobian', 'subproblem0',
     'subproblem1', 'subproblem2', 'subproblem3', 'subproblem4', 'apply_robot_aux_transforms', 
     'unapply_robot_aux_transforms', 'identity_transform', 'random_R', 'random_p', 'random_transform', 
-    'robot6_sphericalwrist_invkin', 'ur_invkin', 'equivalent_configurations', 'iterative_invkin']
+    'robot6_sphericalwrist_invkin', 'ur_invkin', 'equivalent_configurations', 'iterative_invkin', 'slerp']
 
diff --git a/src/general_robotics_toolbox/general_robotics_toolbox.py b/src/general_robotics_toolbox/general_robotics_toolbox.py
index 996c7fc..bea5303 100644
--- a/src/general_robotics_toolbox/general_robotics_toolbox.py
+++ b/src/general_robotics_toolbox/general_robotics_toolbox.py
@@ -298,6 +298,38 @@ def R2rpy(R):
         
     return (r,p,y)    
 
+# Add slerp function using [w,x,y,z] quaternion representation (github copilot)
+def slerp(q0, q1, t):
+    """
+    Spherical linear interpolation between two quaternions
+    
+    :type     q0: numpy.array
+    :param    q0: 4 x 1 vector representation of a quaternion q = [q0;qv]
+    :type     q1: numpy.array
+    :param    q1: 4 x 1 vector representation of a quaternion q = [q0;qv]
+    :type     t: number
+    :param    t: interpolation parameter in the range [0,1]
+    :rtype:   numpy.array
+    :returns: the 4 x 1 interpolated quaternion
+    """
+    
+    assert (t >= 0 and t <= 1), "t must be in the range [0,1]"
+    
+    q0 = q0/np.linalg.norm(q0)
+    q1 = q1/np.linalg.norm(q1)
+    
+    if (np.dot(q0,q1) < 0):
+        q0 = -q0
+    
+    theta = np.arccos(np.dot(q0,q1))
+    
+    if (np.abs(theta) < 1e-6):
+        return q0
+    
+    q = (np.sin((1-t)*theta)*q0 + np.sin(t*theta)*q1)/np.sin(theta)
+    
+    return q/np.linalg.norm(q)
+
 class Robot(object):
     """
     Holds the kinematic information for a single chain robot
diff --git a/test/test_general_robotics_toolbox.py b/test/test_general_robotics_toolbox.py
index 7c3a798..a280621 100755
--- a/test/test_general_robotics_toolbox.py
+++ b/test/test_general_robotics_toolbox.py
@@ -190,6 +190,35 @@ def test_rpy2R():
         rox.R2rpy(R3)
 
 
+def _test_slerp_params(k1,theta1,k2,theta2,t):
+    R1 = rox.rot(k1, theta1)
+    R2 = rox.rot(k2, theta2)
+
+    q1 = rox.R2q(R1)
+    q2 = rox.R2q(R2)
+
+    R1_2 = R1.T @ R2
+    k,theta = rox.R2rot(R1_2)
+
+    R_t = R1 @ rox.rot(k, theta*t)
+    q_t = rox.slerp(q1, q2, t)
+    R_qt = rox.q2R(q_t)
+
+    np.testing.assert_allclose(R_t, R_qt)
+
+    q_t2 = rox.slerp(q1, -q2, t)
+    R_qt2 = rox.q2R(q_t2)
+
+    np.testing.assert_allclose(R_t, R_qt2)
+
+def test_slerp():
+    _test_slerp_params([0.53436371, 0.24406035, 0.80925273], -4.643, 
+                       [-0.71076104,  0.57683297,  0.40259467], 0.25945178, 0.72)
+    _test_slerp_params([0,0,1],0,[0,0,1],1e-7,0.5)
+    _test_slerp_params([0,0,1],1.82,[1,0,0],18.7,0.76)
+    
+
+
 def test_fwdkin():
     
     #TODO: other joint types