From 51a77f1c767714a75dd701bfaf4ee95ea5ac0958 Mon Sep 17 00:00:00 2001
From: Kenneth Yang <82800265+kjy5@users.noreply.github.com>
Date: Thu, 11 Apr 2024 17:03:36 -0700
Subject: [PATCH] Patch new scale convert (#340)

* Update conversion to Vector4

* Ignore W axis in move command
---
 src/ephys_link/platforms/new_scale_handler.py | 28 +++++++++----------
 .../platforms/new_scale_manipulator.py        |  2 +-
 2 files changed, 15 insertions(+), 15 deletions(-)

diff --git a/src/ephys_link/platforms/new_scale_handler.py b/src/ephys_link/platforms/new_scale_handler.py
index 2c43c95..5172286 100644
--- a/src/ephys_link/platforms/new_scale_handler.py
+++ b/src/ephys_link/platforms/new_scale_handler.py
@@ -112,30 +112,30 @@ def _set_can_write(self, request: CanWriteRequest) -> BooleanStateResponse:
         com.dprint(f"[SUCCESS]\t Set can_write state for manipulator {request.manipulator_id}\n")
         return BooleanStateResponse(state=request.can_write)
 
-    def _platform_space_to_unified_space(self, platform_position: list[float]) -> list[float]:
+    def _platform_space_to_unified_space(self, platform_position: Vector4) -> Vector4:
         # unified   <-  platform
         # +x        <-  -x
         # +y        <-  +z
         # +z        <-  +y
         # +d        <-  -d
 
-        return [
-            self.dimensions[0] - platform_position[0],
-            platform_position[2],
-            platform_position[1],
-            self.dimensions[2] - platform_position[3],
-        ]
+        return Vector4(
+            x=self.dimensions.x - platform_position.x,
+            y=platform_position.z,
+            z=platform_position.y,
+            w=self.dimensions.z - platform_position.w,
+        )
 
-    def _unified_space_to_platform_space(self, unified_position: list[float]) -> list[float]:
+    def _unified_space_to_platform_space(self, unified_position: Vector4) -> Vector4:
         # platform  <-  unified
         # +x        <-  -x
         # +y        <-  +z
         # +z        <-  +y
         # +d        <-  -d
 
-        return [
-            self.dimensions[0] - unified_position[0],
-            unified_position[2],
-            unified_position[1],
-            self.dimensions[2] - unified_position[3],
-        ]
+        return Vector4(
+            x=self.dimensions.x - unified_position.x,
+            y=unified_position.z,
+            z=unified_position.y,
+            w=self.dimensions.z - unified_position.w,
+        )
diff --git a/src/ephys_link/platforms/new_scale_manipulator.py b/src/ephys_link/platforms/new_scale_manipulator.py
index e0aac2f..de6b4db 100644
--- a/src/ephys_link/platforms/new_scale_manipulator.py
+++ b/src/ephys_link/platforms/new_scale_manipulator.py
@@ -163,7 +163,7 @@ async def goto_pos(self, request: GotoPositionRequest) -> PositionalResponse:
 
             # Return error if movement did not reach target.
             if not all(
-                abs(axis) < self._movement_tolerance for axis in vector4_to_array(final_position - request.position)
+                abs(axis) < self._movement_tolerance for axis in vector4_to_array(final_position - request.position)[:3]
             ):
                 com.dprint(f"[ERROR]\t\t Manipulator {self._id} did not reach target position.")
                 com.dprint(f"\t\t\t Expected: {request.position}, Got: {final_position}")