From 50ace03943f77398e69e20a14c847f2619be9c74 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anders=20Chirico=20Indreb=C3=B8?= <ACIN@equinor.com>
Date: Wed, 21 Feb 2024 07:45:30 +0100
Subject: [PATCH] Overwrite str function for pose models

---
 src/alitra/models/orientation.py |  6 ++++
 src/alitra/models/pose.py        |  6 ++++
 src/alitra/models/position.py    |  6 ++++
 temp.py                          | 50 ++++++++++++++++++++++++++++++++
 4 files changed, 68 insertions(+)
 create mode 100644 temp.py

diff --git a/src/alitra/models/orientation.py b/src/alitra/models/orientation.py
index d449385..8d34596 100644
--- a/src/alitra/models/orientation.py
+++ b/src/alitra/models/orientation.py
@@ -91,3 +91,9 @@ def from_rotation(rotation: Rotation, frame: Frame) -> Orientation:
         :return: Orientation object
         """
         return Orientation(*rotation.as_quat(), frame=frame)  # type: ignore
+
+    def __str__(self):
+        """
+        :return: Unique string representation of the orientation, ignoring the frame
+        """
+        return "[" + str(self.x) + "," + str(self.y) + "," + str(self.z) + "," + str(self.w) + "]"
diff --git a/src/alitra/models/pose.py b/src/alitra/models/pose.py
index 095a662..96e3b68 100644
--- a/src/alitra/models/pose.py
+++ b/src/alitra/models/pose.py
@@ -41,3 +41,9 @@ def from_array(pos_array: np.ndarray, quat_array: np.ndarray, frame: Frame) -> P
             Orientation.from_quat_array(quat_array, frame),
             frame,
         )
+
+    def __str__(self):
+        """
+        :return: Unique string representation of the pose
+        """
+        return "pos:" + str(self.position) + ", ori: " + str(self.orientation)
diff --git a/src/alitra/models/position.py b/src/alitra/models/position.py
index 4616d85..5861989 100644
--- a/src/alitra/models/position.py
+++ b/src/alitra/models/position.py
@@ -71,3 +71,9 @@ def from_array(position_array: np.ndarray, frame: Frame) -> Positions:
                 Position(x=position[0], y=position[1], z=position[2], frame=frame)
             )
         return Positions(positions=positions, frame=frame)
+
+    def __str__(self):
+        """
+        :return: Unique string representation of the position, ignoring the frame
+        """
+        return "(" + str(self.x) + "," + str(self.y) + "," + str(self.z) + ")"
diff --git a/temp.py b/temp.py
new file mode 100644
index 0000000..f6ca2ca
--- /dev/null
+++ b/temp.py
@@ -0,0 +1,50 @@
+from pathlib import Path
+ 
+import numpy as np
+ 
+from alitra import Frame, MapAlignment, Position, Transform, Translation, align_maps
+from alitra.transform import Rotation
+ 
+here = Path(__file__).parent.resolve()
+
+'''
+0.046175 0.998933 0.000000 246.629578 
+-0.998933 0.046175 0.000000 322.594238 
+0.000000 0.000000 1.000000 0.00000 
+0.000000 0.000000 0.000000 1.000000
+'''
+# E247453mm,N322511mm,U36001mm -> x=0.11907593905116448, y=0.8190074941458545, z=-0.176361
+# E228668mm,N316922mm,U36052mm -> x=4.8833662578143295, y=-18.191902104254204, z=-0.12536099999999806
+# E237508mm,N295502mm,U37667mm -> x=26.66857454840731, y=-10.294643102836075, z=1.489639000000004
+# Path(here.joinpath("./test_data/test_map_robot.json"))
+# map_path = Path(here.joinpath("./jsv1_p1_ap430_east.json"))
+# map_alignment: MapAlignment = MapAlignment.from_config(map_path)
+asset_frame = Frame("asset")
+robot_frame = Frame("robot")
+# transform: Transform = align_maps(map_alignment.map_from, map_alignment.map_to, "z")
+rotation = Rotation.from_euler(seq="ZYX", angles=np.array([-87.5, 0, 0]), degrees=True)
+translation: Translation = Translation(
+    x=246.629578, y=322.594238, z=36.177361, from_=robot_frame, to_=asset_frame
+)
+transform = Transform(
+    translation=translation, from_=robot_frame, to_=asset_frame, rotation=rotation
+)
+test_point: Position = Position(x=237.508, y=295.502, z=37.667, frame=asset_frame)
+asd = transform.transform_position(test_point, from_=asset_frame, to_=robot_frame)
+print(asd)
+ 
+# 20185.046875, y=5239.305176, z=14.383026 -> 0 0 0
+# E20179544mm,N5241228mm,U14971mm
+# E20179402mm,N5245235mm,U14978mm
+# E20183271mm,N5234000mm,U14413mm
+# E20185765mm,N5234008mm,U15981mm
+ 
+ 
+# P1
+# E20185834mm,N5256737mm,U14978mm
+# P2
+# E20179285mm,N5250001mm,U14654mm
+# P3
+# E20179233mm,N5230268mm,U14980mm
+# P4
+# E20189729mm,N5228768mm,U14653mm