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_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_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_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_euler_array(robot_frame):
with pytest.raises(ValueError):
Orientation.from_euler_array(np.array([1, 1]), frame=robot_frame)
def test_orientation_euler_array(robot_frame):
expected_euler = np.array([1, 1, 1])
orientation: Orientation = Orientation.from_euler_array(
expected_euler, robot_frame)
assert np.allclose(orientation.to_euler_array(), expected_euler)
def default_orientation(robot_frame):
return Orientation.from_quat_array(np.array([0, 0, 0, 1]),
frame=robot_frame)