def seToSE( x ):
"""
Convert a twist (a rigid velocity, element of se(3)) to a rigid
motion (an element of SE(3))
INPUT:
x -- 6 sequence
OUTPUT:
result -- 4 x 4
"""
x = asarray(x,dtype=float)
if x.shape != (6,):
raise ValueError("shape must be (6,); got %s" % str(x.shape))
#
return expM(screw(x))
def seToSE(x):
"""
Convert a twist (a rigid velocity, element of se(3)) to a rigid
motion (an element of SE(3))
INPUT:
x -- 6 sequence
OUTPUT:
result -- 4 x 4
"""
x = asarray(x, dtype=float)
if x.shape != (6, ):
raise ValueError("shape must be (6,); got %s" % str(x.shape))
#
return expM(screw(x))
if 0: # create perturbed grid
X,Y = meshgrid(arange(-2,3), arange(-2,3))
X = X + randn(*X.shape)/10
Y = Y + randn(*Y.shape)/10
p = c_[X.flatten(), Y.flatten(), zeros(Y.size), ones(Y.size)].T
if 0:
q = randn(2)
tw = asarray(list(-cross([0,0,1],[q[0],q[1],0]))+[0,0,1])
scm = screw(tw)
if 0:
figure(2)
clf()
for th in arange(0,0.2,0.05):
M1 = expM(scm[:3,:3]*th)
p1 = dot(M1,p[:3,:])
plot( p1[0,:], p1[1,:], 'or')
M2 = soToSO(tw[3:]*th)
p2 = dot(M2,p[:3,:])
plot( p2[0,:], p2[1,:], '.b')
plot(q[0],q[1],'dk')
axis('equal')
grid(1)
if 0:
figure(3)
clf()
for th in arange(0,7,0.1):
M1 = expM(scm*th)
p1 = dot(M1,p)
