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)
