def test_transform_position(default_transform, robot_frame, asset_frame):
expected_pos = Position(1, 1, 1, asset_frame)
pos = Position(1, 1, 1, robot_frame)
pos_to = default_transform.transform_position(positions=pos,
from_=robot_frame,
to_=asset_frame)
assert np.allclose(expected_pos.to_array(), pos_to.to_array())
def test_example():
asset_frame = Frame("asset")
expected_pose: Pose = Pose(
Position(x=40, y=40, z=0, frame=asset_frame),
Orientation.from_euler_array(np.array([-np.pi / 2, 0, 0]),
frame=asset_frame),
frame=asset_frame,
)
"""
This test is an example of one way to use alitra for transformations and alignment
between two coordinate frames
Assume we have two maps, './test_data/test_map_config.json' and
'./test_data/test_map_config_asset.json' which represents a map created by a
robot and a map of an asset respectively. First we load the maps as models
"""
here = Path(__file__).parent.resolve()
robot_map_path = Path(here.joinpath("./test_data/test_map_robot.json"))
robot_map: Map = Map.from_config(robot_map_path)
asset_map_path = Path(here.joinpath("./test_data/test_map_asset.json"))
asset_map: Map = Map.from_config(asset_map_path)
"""
Now we create the transform between the two maps, we know that the only difference
between the two maps are a rotation about the z-axis
"""
transform: Transform = align_maps(robot_map, asset_map, rot_axes="z")
"""
We now create a Pose in the robot frame where the robot is standing
"""
position: Position = Position(x=30, y=40, z=0, frame=robot_map.frame)
orientation: Orientation = Orientation.from_euler_array(
np.array([np.pi, 0, 0]), frame=robot_map.frame)
robot_pose: Pose = Pose(position=position,
orientation=orientation,
frame=robot_map.frame)
"""
Now we can transform the robot_pose into the asset_map to know where on our asset
the robot is
"""
asset_pose = transform.transform_pose(
pose=robot_pose,
from_=robot_pose.frame,
to_=asset_map.frame,
)
assert np.allclose(
expected_pose.orientation.to_euler_array(),
asset_pose.orientation.to_euler_array(),
)
assert np.allclose(expected_pose.position.to_array(),
asset_pose.position.to_array())
def test_align_maps(robot_map, asset_map, default_position, robot_frame, asset_frame):
expected_position: Position = Position(80, 10, 0, frame=asset_frame)
transform = align_maps(map_from=robot_map, map_to=asset_map, rot_axes="z")
position_to = transform.transform_position(
positions=default_position,
from_=robot_frame,
to_=asset_frame,
)
assert np.allclose(expected_position.to_array(), position_to.to_array())
assert expected_position.frame == position_to.frame
def test_no_transformation_when_equal_frames():
p_expected = Position.from_array(np.array([1, 2, 3]), frame=Frame("robot"))
translation = Translation(x=2,
y=3,
from_=Frame("robot"),
to_=Frame("asset"))
transform = Transform.from_euler_array(
euler=np.array([1.0, 0, 0]),
translation=translation,
from_=translation.from_,
to_=translation.to_,
)
p_robot = Position.from_array(np.array([1, 2, 3]), frame=Frame("robot"))
position = transform.transform_position(p_robot,
from_=Frame("robot"),
to_=Frame("robot"))
assert position.frame == p_expected.frame
assert np.allclose(p_expected.to_array(), position.to_array())
def test_transform_pose(default_transform, default_pose, default_rotation,
robot_frame, asset_frame):
expected_pose: Pose = Pose(
position=Position(x=0, y=0, z=0, frame=asset_frame),
orientation=Orientation.from_rotation(rotation=default_rotation,
frame=asset_frame),
frame=asset_frame,
)
pose_to: Pose = default_transform.transform_pose(
pose=default_pose,
from_=robot_frame,
to_=asset_frame,
)
assert np.allclose(expected_pose.orientation.to_quat_array(),
pose_to.orientation.to_quat_array())
assert np.allclose(expected_pose.position.to_array(),
pose_to.position.to_array())
assert expected_pose.frame == pose_to.frame
def test_position_array(robot_frame):
expected_array = np.array([1, 1, 1])
position: Position = Position.from_array(expected_array, frame=robot_frame)
assert np.allclose(expected_array, position.to_array())
def test_position_invalid_array(robot_frame):
with pytest.raises(ValueError):
Position.from_array(np.array([1, 1]), frame=robot_frame)
def test_eq_bounds(default_bounds, robot_frame):
expected_bounds = Bounds(
Position(0, 0, 0, robot_frame), Position(1, 1, 1, robot_frame)
)
assert default_bounds == expected_bounds
def test_position_wrong_frame(default_bounds, asset_frame):
pos = Position(0.5, 0.5, 0.5, asset_frame)
with pytest.raises(ValueError):
default_bounds.position_within_bounds(pos)
def test_position_outside_bounds(default_bounds, robot_frame):
pos = Position(11, 11, 11, robot_frame)
assert default_bounds.position_within_bounds(pos) == False
def test_position_within_bounds(default_bounds, robot_frame):
pos = Position(0.5, 0.5, 0.5, robot_frame)
assert default_bounds.position_within_bounds(pos) == True
def robot_position_2(robot_frame):
return Position(x=1, y=1, z=1, frame=robot_frame)
def robot_position_1(robot_frame):
return Position(x=0, y=0, z=0, frame=robot_frame)
def default_position(robot_frame):
return Position(x=0, y=0, z=0, 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,
),