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 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)
def testExp(self):
(w1,w2,w3,t) = symbols('w1 w2 w3 t')
ct = cos(t)
st = sin(t)
w = array([[w1,w2,w3]]).T
ex = array([[ 1.0 +(1-ct)*(-w2**2-w3**2), w1*w2*(1-ct)-w3*st, w1*w3*(1-ct)+w2*st],
[w1*w2*(1-ct)+w3*st, 1.0 +(1-ct)*(-w1**2-w3**2), w2*w3*(1-ct)-w1*st],
[w1*w3*(1-ct)-w2*st, w2*w3*(1-ct)+w1*st, 1.0 +(1-ct)*(-w1**2-w2**2)]
])
exm = exp(w,t)
assert array_equal(ex,exm)
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)