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