def zero():
return Quaternion(0, sophus.Vector3(0, 0, 0))
def identity():
return Quaternion(1, sophus.Vector3(0, 0, 0))
def simplify(self):
v = sympy.simplify(self.vec)
return Quaternion(sympy.simplify(self.real),
sophus.Vector3(v[0], v[1], v[2]))
def hat(v):
upsilon = sophus.Vector3(v[0], v[1], v[2])
omega = sophus.Vector3(v[3], v[4], v[5])
return sophus.So3.hat(omega).\
row_join(upsilon).\
col_join(sophus.Matrix.zeros(1, 4))
def vee(Omega):
v = sophus.Vector3(
Omega.row(2).col(1),
Omega.row(0).col(2),
Omega.row(1).col(0))
return v