def test_align_positions_not_enough_positions_one_rotation(robot_frame, asset_frame):
positions_from = Positions.from_array(np.array([[1, 0, 0]]), frame=robot_frame)
positions_to = Positions.from_array(np.array([[1, 0, 0]]), frame=asset_frame)
with pytest.raises(ValueError):
align_positions(
positions_from=positions_from, positions_to=positions_to, rot_axes="z"
)
def test_align_positions_translation_only():
expected_rotation = np.array([0, 0, 0])
expected_translation = np.array([1, -1, 0])
robot_frame = Frame("robot")
robot_positions: Positions = Positions.from_array(
np.array(
[
[0, 1, 0],
[1, 1, 0],
[0, 2, 0],
]
),
frame=robot_frame,
)
asset_frame = Frame("asset")
asset_positions: Positions = Positions.from_array(
np.array(
[
[1, 0, 0],
[2, 0, 0],
[1, 1, 0],
]
),
frame=asset_frame,
)
transform: Transform = align_positions(
positions_from=robot_positions, positions_to=asset_positions, rot_axes="xyz"
)
assert np.allclose(expected_rotation, transform.rotation.as_euler("ZYX"))
assert np.allclose(expected_translation, transform.translation.to_array())
def test_align_positions_outside_rsme_treshold(robot_frame, asset_frame):
positions_from = Positions.from_array(
np.array([[20, 10, 0], [60, 20, 0], [80, 70, 0]]), frame=robot_frame
)
positions_to = Positions.from_array(
np.array([[10, 20, 0], [30, 40, 0], [50, 60, 0]]), frame=asset_frame
)
with pytest.raises(ValueError):
align_positions(
positions_from=positions_from, positions_to=positions_to, rot_axes="z"
)
def test_align_positions_unequal_position_length(robot_frame, asset_frame):
positions_from = Positions.from_array(
np.array([[1, 0, 0], [1, 0, 0], [1, 0, 0]]), frame=robot_frame
)
positions_to = Positions.from_array(
np.array([[1, 0, 0], [1, 0, 0]]), frame=asset_frame
)
with pytest.raises(ValueError):
align_positions(
positions_from=positions_from,
positions_to=positions_to,
rot_axes="xyz",
)
def test_transform_position_error(from_, to_, error_expected):
p_robot = Positions.from_array(
np.array([
[1, 2, 3],
[-1, -2, -3],
[0, 0, 0],
[100000, 1, -100000],
], ),
frame=Frame("robot"),
)
euler = np.array([0, 0, 0])
translation = Translation(x=0,
y=0,
from_=Frame("robot"),
to_=Frame("asset"))
transform = Transform.from_euler_array(euler=euler,
translation=translation,
from_=Frame("robot"),
to_=Frame("asset"))
if error_expected:
with pytest.raises(ValueError):
transform.transform_position(p_robot, from_=from_, to_=to_)
else:
transform.transform_position(p_robot, from_=from_, to_=to_)
def test_align_positions_three_dimensions():
expected_rotation = np.array([np.pi / 2, np.pi / 4, np.pi / 2])
expected_translation = np.array([0, 0, 0])
hyp = np.sqrt(1 / 2)
robot_frame = Frame("robot")
robot_positions: Positions = Positions.from_array(
np.array(
[
[1, 0, 0],
[1, 1, 0],
[0, 1, 0],
]
),
frame=robot_frame,
)
asset_frame = Frame("asset")
asset_positions: Positions = Positions.from_array(
np.array(
[
[0, 0, 1],
[-hyp, -hyp, 1],
[-hyp, -hyp, 0],
]
),
frame=asset_frame,
)
transform: Transform = align_positions(
positions_from=robot_positions, positions_to=asset_positions, rot_axes="xyz"
)
assert np.allclose(expected_rotation, transform.rotation.as_euler("zyx"))
assert np.allclose(expected_translation, transform.translation.to_array())
def test_transform_list_of_positions(euler_array, translation,
expected_position):
p_robot = Positions.from_array(
np.array([
[1, 2, 3],
[-1, -2, -3],
[0, 0, 0],
[100000, 1, -100000],
], ),
frame=Frame("robot"),
)
transform = Transform.from_euler_array(
euler=euler_array,
translation=translation,
from_=translation.from_,
to_=translation.to_,
)
p_asset = transform.transform_position(p_robot,
from_=Frame("robot"),
to_=Frame("asset"))
assert p_asset.frame == expected_position.frame
assert np.allclose(expected_position.to_array(), p_asset.to_array())
def test_positions_invalid_array(robot_frame):
with pytest.raises(ValueError):
Positions.from_array(np.array([[1, 1], [1, 1]]), frame=robot_frame)
def test_position_list_array(robot_frame):
expected_array = np.array([[1, 1, 1], [2, 2, 2]])
position_list: Positions = Positions.from_array(expected_array, frame=robot_frame)
assert np.allclose(expected_array, position_list.to_array())
def robot_positions(robot_position_1, robot_position_2, robot_frame):
return Positions([robot_position_1, robot_position_2], frame=robot_frame)
from pathlib import Path
import pytest
from alitra import Bounds, Frame, Map, MapAlignment, Position, Positions
robot_frame = Frame("robot")
expected_map = Map(
name="test_map_robot",
reference_positions=Positions(
positions=[
Position(x=20, y=10, z=0, frame=robot_frame),
Position(x=60, y=20, z=0, frame=robot_frame),
Position(x=80, y=70, z=0, frame=robot_frame),
],
frame=robot_frame,
),
frame=robot_frame,
)
expected_map_bounds = Map(
name="test_map_bounds",
reference_positions=Positions(
positions=[
Position(x=20, y=10, z=0, frame=robot_frame),
Position(x=60, y=20, z=0, frame=robot_frame),
Position(x=80, y=70, z=0, frame=robot_frame),
],
frame=robot_frame,
),
import pytest
from alitra import Frame, Orientation, Pose, Position, Positions, Transform, Translation
@pytest.mark.parametrize(
"euler_array, translation, expected_position",
[
(
np.array([0, 0, 0]),
Translation(x=0, y=0, from_=Frame("robot"), to_=Frame("asset")),
Positions.from_array(
np.array([
[1, 2, 3],
[-1, -2, -3],
[0, 0, 0],
[100000, 1, -100000],
]),
frame=Frame("asset"),
),
),
(
np.array([np.pi * -0.0, 0, 0]),
Translation(x=10, y=10, from_=Frame("robot"), to_=Frame("asset")),
Positions.from_array(
np.array([
[11, 12, 3],
[9, 8, -3],
[10, 10, 0],
[100010, 11, -100000],
]),