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)