fix: drivers follow the units and command contract

CONTRIBUTING.md

@@ -7,13 +7,20 @@ This document outlines the core conventions and control flow to maintain consist
 
 To avoid confusion and unexpected behavior when interfacing with different robot models and manufacturers, **all user-facing inputs and outputs use the following standard units:**
 
-| Type                   | Unit                     |
-| ---------------------- | ------------------------ |
-| **Joint Angle**        | radians                  |
-| **Joint Velocity**     | radians/second           |
-| **Joint Acceleration** | radians/second²          |
-| **Cartesian Position** | meters (`[x, y, z]`)     |
-| **Cartesian Rotation** | radians (`[rx, ry, rz]`) |
+| Type                              | Unit                         |
+| --------------------------------- | ---------------------------- |
+| **Joint Angle** (revolute)        | radians                      |
+| **Joint Velocity** (revolute)     | radians/second               |
+| **Joint Acceleration** (revolute) | radians/second²              |
+| **Joint Position** (linear)       | meters                       |
+| **Joint Velocity** (linear)       | meters/second                |
+| **Joint Acceleration** (linear)   | meters/second²               |
+| **Cartesian Position**            | meters (`[x, y, z]`)         |
+| **Cartesian Rotation**            | radians (`[rx, ry, rz]`)     |
+| **Cartesian Linear Velocity**     | meters/second                |
+| **Cartesian Linear Acceleration** | meters/second²               |
+| **Cartesian Angular Velocity**    | radians/second               |
+| **Time / Duration**               | seconds                      |
 
 > 💡 Always convert incoming or outgoing data to these units at the API boundaries.
 

README.md

@@ -49,6 +49,20 @@ The `armctl` library currently supports the following manufacturers and robot mo
       <img src="https://raw.githubusercontent.com/MGross21/armctl/main/assets/gifs/elephant_pro600.gif" alt="Elephant myCobot Pro600" width="250">
     </td>
   </tr>
+  <tr>
+    <td align="center">
+      <a href="https://www.zaber.com"><b>Zaber</b></a>
+    </td>
+    <td></td>
+    <td></td>
+  </tr>
+  <tr>
+    <td align="center">
+      <sub>Linear Stages</sub>
+    </td>
+    <td></td>
+    <td></td>
+  </tr>
 </table>
 
 > **Want to see your robot supported?**  
@@ -203,11 +217,20 @@ The "home" position refers to a predefined, manufacturer-specific pose that is s
 
 All inputs and outputs use these standard units:
 
-| Type                   | Unit                     |
-| ---------------------- | ------------------------ |
-| **Joint Angle**        | radians                  |
-| **Cartesian Position** | meters (`[x, y, z]`)     |
-| **Cartesian Rotation** | radians (`[rx, ry, rz]`) |
+| Type                              | Unit                         |
+| --------------------------------- | ---------------------------- |
+| **Joint Angle** (revolute)        | radians                      |
+| **Joint Velocity** (revolute)     | radians/second               |
+| **Joint Acceleration** (revolute) | radians/second²              |
+| **Joint Position** (linear)       | meters                       |
+| **Joint Velocity** (linear)       | meters/second                |
+| **Joint Acceleration** (linear)   | meters/second²               |
+| **Cartesian Position**            | meters (`[x, y, z]`)         |
+| **Cartesian Rotation**            | radians (`[rx, ry, rz]`)     |
+| **Cartesian Linear Velocity**     | meters/second                |
+| **Cartesian Linear Acceleration** | meters/second²               |
+| **Cartesian Angular Velocity**    | radians/second               |
+| **Time / Duration**               | seconds                      |
 
 ### Connection Template
 
@@ -302,7 +325,9 @@ armctl --help
 - [JAKA](https://www.jaka.com/en)
   - **Models:** Zu 5
 - [Dobot](https://www.dobot-robots.com)
-  - **Models:** Magician Lite
+  - **Models:** Magician Lite, M1 Pro
+- [ABB](https://new.abb.com/products/robotics)
+  - **Controllers:** IRC5, OmniCore (via Robot Web Services)
 
 ## Future Development
 

armctl/_blank/robot.py

@@ -16,7 +16,6 @@
 # Universal Imports
 from __future__ import annotations
 
-import atexit
 import math
 
 from armctl.templates import Commands, Properties
@@ -47,7 +46,6 @@ def connect(self):
         super().connect()  # Call first to ensure base connection logic is executed
         # Additional connection logic can be added here if needed
 
-    @atexit.register
     def disconnect(self):
         # Additional disconnection logic can be added here if needed
         super().disconnect()  # Call last to ensure base disconnection logic is executed
@@ -61,7 +59,7 @@ def sleep(self, seconds: float) -> None:
 
     def move_joints(self, pos: list[float]) -> None:
         """Move robot joints to specified positions (in radians or degrees)."""
-        cc.move_joints(pos)
+        cc.move_joints(self, pos)
         # Additional move logic can be added here if needed
         # Send move command to robot
         pass
@@ -75,7 +73,7 @@ def get_joint_positions(self) -> list[float]:
 
     def move_cartesian(self, pose: list[float]) -> None:
         """Move robot to specified cartesian pose [x, y, z, rx, ry, rz]."""
-        cc.move_cartesian(pose)
+        cc.move_cartesian(self, pose)
         # Additional move logic can be added here if needed
         # Send move command to robot
         pass

armctl/elephant_robotics/elephant_robotics.py

@@ -52,60 +52,63 @@ def sleep(self, seconds):
         self.send_command(f"wait({seconds})")
         time.sleep(seconds)
 
-    def move_joints(self, pos: list[float], speed: int = uu.deg2rad(500)) -> None:
+    def move_joints(self, pos: list[float], speed: float = uu.deg2rad(500)) -> None:
         """
         Move the robot to the specified joint positions.
 
         Parameters
         ----------
         pos : list of float
             Joint positions in radians [j1, j2, j3, j4, j5, j6].
-        speed : int, optional
+        speed : float, optional
             Speed of the movement, range `0` ~ `math.radians(2000)` (default: `math.radians(500)`).
         """
 
         cc.move_joints(self, pos, speed)
 
         pos_deg = uu.joints2deg(pos)
-        speed_deg = uu.rad2deg(speed)
+        speed_deg = round(uu.rad2deg(speed))
 
         command = f"set_angles({','.join(map(str, pos_deg))},{speed_deg})"
         response = self.send_command(command)
 
         assert response == f"{command}:[ok]", f"Failed to move joints: {response}"
 
-        while any(abs(a - b) > 3 for a, b in zip(self.get_joint_positions(), pos)):
+        while any(abs(a - b) > uu.deg2rad(3) for a, b in zip(self.get_joint_positions(), pos)):
             time.sleep(1)
 
     def move_cartesian(
         self,
-        pose: tuple[float, float, float, float, float, float],
-        speed: int = uu.deg2rad(500),
+        pose: list[float],
+        speed: float = uu.deg2rad(500),
     ) -> None:
         """
         Move the robot to the specified Cartesian coordinates.
 
         Parameters
         ----------
-        pose : tuple of float
+        pose : list of float
             Cartesian coordinates in the format `[x, y, z, rx, ry, rz]`.
-        speed : int, optional
+        speed : float, optional
             Speed of the movement, range `0` ~ `math.radians(2000)` (default: `math.radians(500)`).
         """
 
         cc.move_cartesian(self, pose)
 
-        pose_deg = uu.pose2deg(pose)
-        speed_deg = uu.rad2deg(speed)
+        pose_mm_deg = [uu.m2mm(v) for v in pose[:3]] + uu.pose2deg(pose)[3:]
+        speed_deg = round(uu.rad2deg(speed))
 
-        command = f"set_coords({','.join(map(str, pose_deg))},{speed_deg})"
+        command = f"set_coords({','.join(map(str, pose_mm_deg))},{speed_deg})"
 
         assert self.send_command(command) == "set_coords:[ok]"
 
-        while not all(abs(a - b) <= 1 for a, b in zip(self.get_cartesian_position(), pose)):
+        tolerances = [1e-3] * 3 + [uu.deg2rad(1)] * 3
+        while any(
+            abs(a - b) > tol for a, b, tol in zip(self.get_cartesian_position(), pose, tolerances)
+        ):
             time.sleep(1)
 
-    def get_joint_positions(self):
+    def get_joint_positions(self) -> list[float]:
         response = self.send_command("get_angles()")
         if response == "[-1.0, -2.0, -3.0, -4.0, -1.0, -1.0]":
             raise ValueError("Invalid joint positions response from robot")
@@ -115,9 +118,9 @@ def get_joint_positions(self):
                 response[response.index("[") + 1 : response.index("]")].split(","),
             )
         )  # From string list to float list
-        return [round(x, 2) for x in joint_positions]
+        return uu.joints2rad([round(x, 2) for x in joint_positions])
 
-    def get_cartesian_position(self):
+    def get_cartesian_position(self) -> list[float]:
         response = self.send_command("get_coords()")  # [x, y, z, rx, ry, rz]
         if response == "[-1.0, -2.0, -3.0, -4.0, -1.0, -1.0]":
             raise ValueError("Invalid cartesian position response from robot")
@@ -127,9 +130,10 @@ def get_cartesian_position(self):
                 response[response.index("[") + 1 : response.index("]")].split(","),
             )
         )  # From string list to float list
-        return [round(x, 2) for x in cartesian_position]
+        x, y, z, *rotation = (round(v, 2) for v in cartesian_position)
+        return [uu.mm2m(x), uu.mm2m(y), uu.mm2m(z)] + uu.joints2rad(rotation)
 
-    def stop_motion(self):
+    def stop_motion(self) -> None:
         command = "task_stop"
         response = self.send_command(f"{command}()")
 
@@ -140,9 +144,8 @@ def stop_motion(self):
 
         if result != "[ok]":
             raise SystemError(result)
-        return True
 
-    def get_robot_state(self):
+    def get_robot_state(self) -> bool:
         command = "check_running"
         response = self.send_command(f"{command}()")
 

armctl/elephant_robotics/mycobot.py

@@ -11,4 +11,4 @@ def home(self):
         """
         Move the robot to the home position: `[0, -90, 90, -90, -90, 0]`.
         """
-        self.move_joints(self.HOME_POSITION, speed=750)
+        self.move_joints(self.HOME_POSITION, speed=uu.deg2rad(750))

armctl/universal_robots/manipulators/on_robot.py

@@ -47,14 +47,12 @@ def set_position(self, position: float, name: str = "rg2", force: float = 10) ->
 
     def get_position(self, name: str = "rg2") -> float:
         """Get the current position of the gripper."""
-        response = self.send_command(
-            f"{name}_get_width()\n", suppress_output=True, raw_response=True
-        )
+        response = self.send_command(f"{name}_get_width()\n", suppress_output=True)
         return float(response.strip())
 
     def get_status(self) -> bool:
         """Get the current status of the gripper."""
-        response = self.send_command("rg2_get_status()\n", suppress_output=True, raw_response=True)
+        response = self.send_command("rg2_get_status()\n", suppress_output=True)
         return response.strip() == "1"  # 1 means gripper is closed, 0 means open
 
     def open(self, force: float = 20) -> None: