def testLogTranslation(self): #translation p = array([[2,1,1]]).T T= composeTrans(numpy.eye(3),p) (w,u,theta,h) = log(T) assert array_equal(p,u) and array_equal(w,numpy.zeros((3,1))) and theta == 1 Tm = exp(composeScrew(w, u),theta) assert array_equal(T, Tm)
def testLogRotation(self): t = math.pi/5 ct = math.cos(t) st = math.sin(t) R = array([[1,0,0], [0,ct,-st], [0,st,ct] ]) T= composeTrans(R,numpy.zeros((3,1))) (w,u,theta,h) = log(T) Tm = exp(composeScrew(w, u),theta) assert array_equal(T, Tm)
def testBreakComposeTrans(self): (a,b,c,d,e,f,g,h,i,j,k,l) = symbols('a b c d e f g h i j k l') Tm = array([[a,b,c,d], [e,f,g,h], [i,j,k,l], [0,0,0,1]]) Rm = array([[a,b,c], [e,f,g], [i,j,k]]) pm = array([[d], [h], [l]]) (R,p) = breakTrans(Tm) assert array_equal(R,Rm) and array_equal(p,pm) T = composeTrans(R, p) assert array_equal(T,Tm)
def testAdj(self): #Rotation by z pi/5 and t = math.pi/5 ct = math.cos(t) st = math.sin(t) R = array([[1,0,0], [0,ct,-st], [0,st,ct] ]) #translation p = array([[2,1,1]]).T A = array([ [ 1, 0, 0, 0, 0, 0], [ 0, 0.80902, -0.58779, 0, 0, 0], [ 0, 0.58779, 0.80902, 0, 0, 0], [ 0, -0.22123, 1.39680, 1, 0, 0], [ 1, -1.17557, -1.61803, 0, 0.80902, -0.58779], [-1, 1.61803, -1.17557, 0, 0.58779, 0.80902] ]) Am = adj(composeTrans(R, p)) assert allclose(A,Am)
def testLogRotTrans(self): t = math.pi/5 ct = math.cos(t) st = math.sin(t) R = array([[1,0,0], [0,ct,-st], [0,st,ct] ]) we = array([[1,0,0]]).T Re = exp(we, t) assert array_equal(R, Re) p = array([[2,1,1]]).T T= composeTrans(R,p) (w,u,theta,h) = log(T) Tm = exp(composeScrew(w, u),theta) assert allclose(T,Tm)