def test_make_orientation_rel_2():
parent = Orientation(0, 0, 0, 1)
child = Orientation()
child.set_from_quaternion(quaternion.from_euler_angles(
0.123, 0.345, 0.987))
assert make_orientation_rel(parent, child) == child
def test_make_orientation_rel_and_then_again_abs_2():
parent = Orientation()
parent.set_from_quaternion(quaternion.from_euler_angles(1.25, -2, 3.78))
obj = Orientation()
obj.set_from_quaternion(quaternion.from_euler_angles(-2.2, 4, 1.9))
rel_obj = make_orientation_rel(parent, obj)
assert obj == make_orientation_abs(parent, rel_obj)
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
