def test_invalid_orientation() -> None:
with pytest.raises(Arcor2Exception):
Orientation(0, 0, 0, 0)
o = Orientation()
o.w = 0
with pytest.raises(Arcor2Exception):
o.as_quaternion()
def make_orientation_abs(parent: Orientation, child: Orientation) -> Orientation:
p = Orientation()
p.set_from_quaternion(child.as_quaternion()*parent.as_quaternion().conjugate().inverse())
return p
def make_orientation_rel(parent: Orientation, child: Orientation) -> Orientation:
p = Orientation()
p.set_from_quaternion(child.as_quaternion() / parent.as_quaternion())
return p
def test_valid_orientation() -> None:
o1 = Orientation(0, 0, 0, 1)
o2 = Orientation()
o2.set_from_quaternion(o1.as_quaternion())
assert o1 == o2
