def to_axis_angle(self):
# type: () -> typing.Tuple[Vector3, float]
assert abs(self.w) <= 1.0
denom = numpy.sqrt(1 - self.w * self.w)
angle = 2.0 * numpy.arccos(self.w)
if denom > numpy.finfo(float).eps:
return Vector3(self.vec() / denom), angle
else:
return Vector3(1.0, 0.0, 0.0), angle # Singularity
def test_from_two_vecs(self):
v1 = Vector3(100, 0, 0)
v2 = Vector3(1, 1, 0)
qt = Quaternion.from_two_vectors(v1, v2)
angle = qt.angular_distance(quaternion=Quaternion())
self.assertAlmostEqual(angle * 180.0 / math.pi, 45.0)
v3 = v1 * qt
v3.normalize()
v2.normalize()
self.assertTrue(numpy.isclose(v2.data(), v3.data()).all())
def test_angle_between(self):
v1 = Vector3(100, 0, 0)
v2 = Vector3(0, 1, 0)
angle = v1.angle(v2)
self.assertAlmostEqual(angle, math.pi / 2)
v1 = Vector3(1000, 0, 0)
v2 = Vector3(1, 1, 0)
angle = v1.angle(v2)
self.assertAlmostEqual(angle, math.pi / 4)
def from_two_vectors(cls, vector_a, vector_b):
# type: (Vector3, Vector3) -> Quaternion
a = Vector3(vector_a)
b = Vector3(vector_b)
a.normalize()
b.normalize()
c = a.dot(b)
axis = a.cross(b)
s = math.sqrt((1.0 + c) * 2.0)
vec = axis * (1.0 / s)
return Quaternion(w=s * 0.5, x=vec.x, y=vec.y, z=vec.z)
def from_msg(cls, ros_msg):
# type: (genpy.Message) -> Transform
if isinstance(ros_msg, RosTransform):
return Transform(translation=Vector3.from_msg(ros_msg.translation),
rotation=Quaternion.from_msg(ros_msg.rotation))
elif isinstance(ros_msg, RosPose):
return Transform(translation=Vector3.from_msg(ros_msg.position),
rotation=Quaternion.from_msg(ros_msg.orientation))
else:
raise NotImplementedError(
'Unknown message type to convert to math6D.Transform: {}'.
format(type(ros_msg)))
def __init__(self, *args, **kwargs):
self.__rotation = None
self.__translation = None
if len(args) == 0 and len(kwargs) == 0:
self.__rotation = Quaternion()
self.__translation = Vector3()
elif len(args) == 1:
_arg = args[0]
if isinstance(_arg, genpy.Message):
t = Transform.from_msg(_arg)
self.__rotation = t.rotation
self.__translation = t.translation
else:
raise NotImplementedError(
'Expected Message type, got: {}'.format(type(_arg)))
elif len(args) + len(kwargs) == 2:
if len(args) == 2:
_tr = args[0]
_rot = args[1]
else:
_tr = kwargs[
self.
TRANSLATION_KW] if self.TRANSLATION_KW in kwargs else Vector3(
)
_rot = kwargs[
self.
ROTATION_KW] if self.ROTATION_KW in kwargs else Quaternion(
)
if isinstance(_tr, RosVector3):
self.__translation = Vector3().from_msg(_tr)
elif isinstance(_tr, Vector3):
self.__translation = _tr
else:
self.__translation = Vector3(_tr)
if isinstance(_rot, RosQuaternion):
self.__rotation = Quaternion().from_msg(_rot)
elif isinstance(_rot, Quaternion):
self.__rotation = _rot
else:
self.__rotation = Quaternion(_rot)
else:
raise NotImplementedError(
'Unexpected input args of len: {}'.format(
len(args) + len(kwargs)))
def test_transform_matrix(self):
t = Transform(Vector3(1, 2, 3),
Quaternion.from_euler_extrinsic(0.1, 0.2, 0.3))
m = t.matrix()
t2 = Transform.from_matrix(m)
m2 = t2.matrix()
self.assertTrue(numpy.isclose(m, m2).all())
def test_from_xy(self):
random_qt = Quaternion.from_euler_extrinsic(0.2, 0.3, 0.9)
x_vec = Vector3(1, 0, 0) * random_qt
y_vec = Vector3(0, 1, 0) * random_qt
z_vec = Vector3(0, 0, 1) * random_qt
m = numpy.identity(3)
m[:, 0] = x_vec.data()
m[:, 1] = y_vec.data()
m[:, 2] = z_vec.data()
m = numpy.matrix(m)
new_q = Quaternion.from_matrix(m)
self.assertTrue(
numpy.isclose(new_q.coeffs(), random_qt.coeffs()).all())
def __mul__(self, other):
if isinstance(other, Transform):
t = self.translation + (self.rotation * other.translation)
q = self.rotation * other.rotation
return Transform(t, q)
elif isinstance(other, Vector3):
v = numpy.ones(4)
v[:3] = other.data()
return Vector3(numpy.dot(self.matrix(), v)[:3])
else:
raise NotImplementedError('Cannot multiply type of: {}'.format(
type(other)))
def __mul__(self, other):
if isinstance(other, Vector3):
return Vector3((self * Quaternion(0, other.x, other.y, other.z) *
self.conjugate()).vec())
elif isinstance(other, Quaternion):
w = self.w * other.w - numpy.dot(self.vec(), other.vec())
vec = numpy.cross(
self.vec(),
other.vec()) + self.w * other.vec() + other.w * self.vec()
return Quaternion(w=w, x=vec[0], y=vec[1], z=vec[2])
else:
return NotImplemented
def from_matrix(cls, matrix):
# type: (numpy.matrix) -> Transform
# Check matrix properties
if matrix.shape != (4, 4):
raise ValueError(
'Invalid matrix shape: Input must be a 4x4 matrix')
if not numpy.isclose(numpy.linalg.det(matrix), 1.0):
raise ValueError('Matrix determinant must be +1.0')
q = Quaternion.from_matrix(matrix[0:3, 0:3])
t = Vector3(matrix[0:3, 3].flatten())
return Transform(t, q)
def __rmul__(self, other):
if isinstance(other, Vector3):
return Vector3((self * Quaternion(0, other.x, other.y, other.z) *
self.conjugate()).vec())
else:
return NotImplemented
def to_rotation_vector(self):
# type: () -> Vector3
axis, angle = self.to_axis_angle()
return Vector3(axis * angle)
class Axis(enum.Enum):
AXIS_X = Vector3(1, 0, 0) # type: Axis
AXIS_Y = Vector3(0, 1, 0) # type: Axis
AXIS_Z = Vector3(0, 0, 1) # type: Axis