Added files

Author: sai-karthikeya-vemuri

ax2om.py

@@ -0,0 +1,37 @@
+import numpy as np 
+import pytest
+
+#This function takes a axis angle pair (radians) as a four element vector and converts it into corresponding rotation matrix 
+def ax2om(n):
+    c= np.cos(n[3])
+    s= np.sin(n[3])
+    p=-1
+    A=np.zeros((3,3))
+    A[0][0]= c + (1-c)*(n[0]**2)
+    A[0][1]= (1-c)*n[0]*n[1] + s*n[2]
+    A[0][2]= (1-c)*n[0]*n[2] - s*n[1]
+    A[1][0]= (1-c)*n[0]*n[1] - s*n[2]
+    A[1][1]= c + (1-c)*(n[1]**2)
+    A[1][2]= (1-c)*n[2]*n[1] + s*n[0]
+    A[2][0]= (1-c)*n[0]*n[2] + s*n[1]
+    A[2][1]= (1-c)*n[2]*n[1] - s*n[0]
+    A[2][2]= c + (1-c)*(n[2]**2)
+    return A
+
+def test_case1():
+    n=[0,0,1,0]
+    x= np.eye(3)
+    y=np.array_equal(ax2om(n),x)
+    print(ax2om(n))
+    assert y== True 
+
+
+def test_case2():
+    n=[-1,0,0,np.pi]
+    x= np.eye(3)
+    x[1][1]=-1
+    x[2][2]=-1
+    y=np.array_equal(np.around(ax2om(n),1),x)
+    print(np.around(ax2om(n),2))
+    assert y== True 
+

ax2qu.py

@@ -0,0 +1,19 @@
+import pytest
+import numpy as np 
+# This function converts a axis angle pair in radians into unit quarternions
+
+def ax2qu(n):
+    q=[np.cos(n[3]/2),np.sin(n[3]/2)*n[0],np.sin(n[3]/2)*n[1],np.sin(n[3]/2)*n[2]]
+    return q
+
+
+def test_case1():
+    n=[0,0,1,0]
+    assert ax2qu(n) == [1,0,0,0]
+def test_case2():
+    n =[0,0,-1,0.5*np.pi]
+    x=np.around(ax2qu(n),7)
+    y = np.array([0.7071068,0,0,-0.7071068])
+    z = np.array_equal(x,y)
+    assert z==True
+

ax2ro.py

@@ -0,0 +1,17 @@
+import numpy as np 
+import pytest
+
+#This function takes in axis angle pair in radians and converts it into corresponding rodrigues vector
+
+def ax2ro(n):
+    if n[3]==np.pi:
+        print("rodrigues vector is not possible as tan(90)=infinity")
+    
+    n[3]=np.tan(n[3]/2)
+    return n
+
+def test_case1():
+    n=[0,0,1,0.5*np.pi]
+    x=np.around(ax2ro(n),1)
+
+    assert x[3]==1

eu2ax.py

@@ -0,0 +1,28 @@
+import numpy as np
+#This function takes in the three euler angles in the bunge convention in degrees and gives the corresponding axis angle pair the output is of
+# form (n,alpha) in degrees 
+def eu2ax(a,b,c):
+    a= np.pi*a/180
+    b= np.pi*b/180
+    c= np.pi*c/180
+    t=np.tan(b/2)
+    sigma=(a+c)/2
+    delta=(a-c)/2
+    p=-1
+    tau=np.sqrt((t*t)+(np.sin(sigma)*np.sin(sigma)))
+    alpha=2*np.arctan(tau/np.cos(sigma))
+    output = np.zeros((4))
+    if alpha <= np.pi:
+        output[3]= alpha*180/np.pi
+        output[0]= -p*t*np.cos(delta)/tau
+        output[1]=-p*t*np.sin(delta)/tau
+        output[2]=-p*t*np.sin(sigma)/tau
+    else:
+        output[3]=360- alpha*180/np.pi
+        output[0]= p*t*np.cos(delta)/tau
+        output[1]=p*t*np.sin(delta)/tau
+        output[2]=p*t*np.sin(sigma)/tau
+    return output
+
+
+print(eu2ax(360,90,180))

eu2om.py

@@ -0,0 +1,30 @@
+import numpy as np
+import scipy
+#This function converts given euler angles (Bunge Convention, degrees)into corresponding equivalent rotation matrix
+def eu2om(a,b,d):
+    
+    a= (np.pi)*a/180
+    b= (np.pi)*b/180
+    d= (np.pi)*d/180
+
+
+    A=np.zeros((3,3))
+    c1=np.cos(a)
+    s1=np.sin(a)
+    c2=np.cos(d)
+    s2=np.sin(d)
+    c=np.cos(b)
+    s=np.sin(b)
+    
+    A[0][0]=(c1*c2) - (s1*c*s2)
+    A[0][1]=(s1*c2) + (c1*c*s2)
+    A[0][2]=s*s2
+    A[1][0]=(-c1*s2)-(s1*c*c2)
+    A[1][1]=(-s1*s2)+ (c1*c*c2)
+    A[1][2]=s*c2
+    A[2][0]=s1*s
+    A[2][1]=-c1*s
+    A[2][2]=c
+    return A
+
+print(np.around(eu2om(270,180,0),1))

eu2qu.py

@@ -0,0 +1,17 @@
+import numpy as np 
+#This function takes 3 euler angles in bunge convention in radians and gives a quartenion
+def eu2qu(a1,a2,a3):
+    c = np.cos(a2/2)
+    s=np.sin(a2/2)
+    sigma =(a1+a3)/2
+    delta = (a1-a3)/2
+    p=-1
+    q= [0,0,0,0]
+    q = [c*np.cos(sigma),-1*p*s*np.cos(delta),-1*p*s*np.sin(delta),-1*p*c*np.sin(sigma)]
+    if q[0]<0:
+        q= -1*q
+    return q
+
+print(eu2qu(0,0.25*np.pi,0))
+
+

om2ax.py

@@ -0,0 +1,42 @@
+import math
+import pytest
+import numpy as np 
+from numpy import linalg as LA
+#This function takes in the rotation matrix and gives axis angle pair in radians
+def om2ax(A):
+    p=-1
+    w= math.acos(0.5*((A[0][0]+A[1][1]+A[2][2])-1))
+    if w==0:
+        f = [0,0,1,0]
+    else:
+        values,vectors= LA.eig(A)
+        if values[0]==1:
+            vector=vectors[:,0]
+        elif values[1]==1:
+            vector=vectors[:,1]
+        else:
+            vector= vectors[:,2]
+        if A[2][1]!=A[1][2]:
+            vector[0]=sign(p*(A[2][1]-A[1][2]))*abs(vector[0])
+        if A[0][2]!=A[2][0]:
+            vector[1]=sign(p*(A[0][2]-A[2][0]))*abs(vector[1])
+        if A[1][0]!=A[0][1]:
+            vector[2]=sign(p*(A[1][0]-A[0][1]))*abs(vector[2])
+        f = [vector[0],vector[1],vector[2],w]
+    return f
+
+
+
+
+def test_case1():
+    A=np.eye(3)
+    assert om2ax(A)==[0,0,1,0]
+
+
+def test_case2():
+    A=np.eye(3)
+    A[1][1]=-1
+    A[2][2]=-1
+    assert om2ax(A)==[1,0,0,np.pi]
+
+

om2eu.py

@@ -0,0 +1,29 @@
+import pytest
+
+import numpy as np 
+#This function takes in the rotation matrix and gives euler angles in bunge convention in radians
+
+def om2eu(A):
+    if  abs(A[2][2]) != 1:
+        xhi= 1/np.sqrt(1-(A[2][2]*A[2][2]))
+        theta = [np.arctan2(A[2][0]*xhi,A[2][1]*xhi*-1),np.arccos(A[2][2]),np.arctan2(xhi*A[0][2],xhi*A[1][2])]
+    else:
+        theta = [np.arctan2(A[0][1],A[0][0]),0.5*np.pi*(1-A[2][2]),0]
+    return theta
+
+def test_case1():
+    A= np.eye(3)
+    assert om2eu(A)== [0,0,0]
+  
+def test_case2():
+    A=np.eye(3)
+    A[0][0]=-1
+    A[1][1]=-1
+    print(A)
+    assert om2eu(A)==[2*np.pi,0,np.pi]
+def test_case3():
+    A= np.zeros((3,3))
+    A[0][2]=1
+    A[1][1]=-1
+    A[2][0]=1
+    assert om2eu(A)==[0.5*np.pi,0.5*np.pi,0.5*np.pi]

om2qu.py

@@ -0,0 +1,28 @@
+import pytest
+import numpy as np 
+#This function takes a rotation matrix and gives corresponding quaternion
+def om2qu(A):
+    q0=0.5*np.sqrt(1+A[0][0]+A[1][1]+A[2][2])
+    q1=0.5*np.sqrt(1+A[0][0]-A[1][1]-A[2][2])
+    q2=0.5*np.sqrt(1-A[0][0]+A[1][1]-A[2][2])
+    q3=0.5*np.sqrt(1-A[0][0]-A[1][1]+A[2][2])
+    if A[2][1]<A[1][2]:
+        q1=-1*q1
+    if A[0][2]<A[2][0]:
+        q2=-1*q2
+    if A[1][0]<A[0][1]:
+        q3=-1*q3
+    
+    v = np.sqrt(q0**2+q1**2+q2**2+q3**2)
+    q=[q0/v,q1/v,q2/v,q3/v]
+    return q
+
+def test_case1():
+    A=np.eye(3)
+    assert om2qu(A)==[1,0,0,0]
+def test_case2():
+    A=np.eye(3)
+    A[0][0]=-1
+    A[1][1]=-1
+    assert om2qu(A)==[0,0,0,1]
+

qu2ax.py

@@ -0,0 +1,27 @@
+import math
+import numpy as np 
+import pytest
+#This function converts quarternion into axis angle pair in radians
+def qu2ax(q):
+    w=2*math.acos(q[0])
+    if w==0:
+        n=[0,0,1,0]
+    elif q[0]==0:
+        n=[q[1],q[2],q[3],np.pi]
+    elif q[0]>0:
+        s = 1/np.sqrt(q[1]**2+q[2]**2+q[3]**2)
+        n = [s*q[1],s*q[2],s*q[3],w]
+    else:
+        s = -1/np.sqrt(q[1]**2+q[2]**2+q[3]**2)
+        n = [s*q[1],s*q[2],s*q[3],w]
+    return n
+def test_case1():
+    x = qu2ax([1,0,0,0])
+    y = [0,0,1,0]
+    i = np.array_equal(x,y)
+    assert i == True
+def test_case2():
+    x = qu2ax([0,0,0,1])
+    y = [0,0,1,np.pi]
+    i = np.array_equal(x,y)
+    assert i == True