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)