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)
