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:]
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 se3ToXYZQUAT(M):
'''
Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,Q1,Q2,Q3,Q4] .
'''
xyz = M.translation
quat = se3.Quaternion(M.rotation).coeffs()
return [float(xyz[0, 0]), float(xyz[1, 0]), float(xyz[2, 0]),
float(quat[0, 0]), float(quat[1, 0]), float(quat[2, 0]), float(quat[3, 0])]