def exp6( nu ):
'''
Exp: se3 -> SE3. Return the integral of the input spatial velocity during time 1.
'''
if isinstance(nu,se3.Motion): w = nu.angular; v = nu.linear
else: w = nu[3:]; v = nu[:3]
if npl.norm(w)>1e-15:
R = exp3(w)
t = npl.norm(w)
S = skew(w)
V = eye(3) + (1-cos(t))/t**2 * S + (t-sin(t))/t**3 * S*S
p = V*v
return se3.SE3(R,p)
else:
return se3.SE3(eye(3),v)
def XYZQUATToSe3(xyzq):
'''
Reverse function of se3ToXYZQUAT: convert [X,Y,Z,Q1,Q2,Q3,Q4] to a SE3 element
'''
if isinstance(xyzq, (tuple, list)):
xyzq = np.matrix(xyzq, np.float).T
return se3.SE3(se3.Quaternion(xyzq[6, 0], xyzq[3, 0], xyzq[4, 0], xyzq[5, 0]).matrix(), xyzq[:3])
def increment(self,q,dq):
M = se3.SE3( se3.Quaternion(q[6,0],q[3,0],q[4,0],q[5,0]).matrix(), q[:3])
dM = exp(dq[:6])
M = M*dM
q[:3] = M.translation
q[3:7] = se3.Quaternion(M.rotation).coeffs()
q[7:] += dq[6:]