def testBreakComposeScrew(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') Sm = array([[a,b,c,d,e,f]]).T wm = array([[a,b,c]]).T um = array([[d,e,f]]).T S = composeScrew(wm,um) assert array_equal(S,Sm) (w,u) = breakScrew(Sm) assert array_equal(w,wm) and array_equal(u,um)
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 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)