def test_transform_orientation(default_transform, default_orientation,
robot_frame, asset_frame):
expected_orientation: Orientation = Orientation.from_quat_array(
np.array([0, 0, 0, 1]), frame=asset_frame)
orientation: Orientation = default_transform.transform_orientation(
orientation=default_orientation,
from_=robot_frame,
to_=asset_frame,
)
assert np.allclose(expected_orientation.to_quat_array(),
orientation.to_quat_array())
def test_orientation_quat_array(robot_frame):
expected_array = np.array([0.5, 0.5, 0.5, 0.5])
orientation: Orientation = Orientation.from_quat_array(
expected_array, robot_frame)
assert np.allclose(orientation.to_quat_array(), expected_array)
def test_orientation_invalid_quat_array(robot_frame):
with pytest.raises(ValueError):
Orientation.from_quat_array(np.array([1, 1]), frame=robot_frame)
def default_orientation(robot_frame):
return Orientation.from_quat_array(np.array([0, 0, 0, 1]),
frame=robot_frame)