diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 6f9360f..05dc3ee 100644 --- a/CONTRIBUTING.md +++ b/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. diff --git a/README.md b/README.md index fe2b60e..1f10f09 100644 --- a/README.md +++ b/README.md @@ -49,6 +49,20 @@ The `armctl` library currently supports the following manufacturers and robot mo Elephant myCobot Pro600 + + + Zaber + + + + + + + Linear Stages + + + + > **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 diff --git a/armctl/_blank/robot.py b/armctl/_blank/robot.py index 4cd1fac..915e378 100644 --- a/armctl/_blank/robot.py +++ b/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 diff --git a/armctl/elephant_robotics/elephant_robotics.py b/armctl/elephant_robotics/elephant_robotics.py index 98c0608..c5c1b41 100644 --- a/armctl/elephant_robotics/elephant_robotics.py +++ b/armctl/elephant_robotics/elephant_robotics.py @@ -52,7 +52,7 @@ 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. @@ -60,52 +60,55 @@ def move_joints(self, pos: list[float], speed: int = uu.deg2rad(500)) -> None: ---------- 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}()") diff --git a/armctl/elephant_robotics/mycobot.py b/armctl/elephant_robotics/mycobot.py index be98946..31df209 100644 --- a/armctl/elephant_robotics/mycobot.py +++ b/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)) diff --git a/armctl/universal_robots/manipulators/on_robot.py b/armctl/universal_robots/manipulators/on_robot.py index 656330f..50d4b3a 100644 --- a/armctl/universal_robots/manipulators/on_robot.py +++ b/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: diff --git a/armctl/universal_robots/universal_robots.py b/armctl/universal_robots/universal_robots.py index 01126d4..a1898bd 100644 --- a/armctl/universal_robots/universal_robots.py +++ b/armctl/universal_robots/universal_robots.py @@ -47,6 +47,12 @@ def disconnect(self): self.rtde = None super().disconnect() + @property + def _rtde(self) -> RTDE: + if self.rtde is None: + raise ConnectionError("Robot is not connected.") + return self.rtde + def sleep(self, seconds: float) -> None: cc.sleep(seconds) self.send_command(f"sleep({seconds})\n") @@ -82,10 +88,10 @@ def move_joints( f" a={acceleration}, v={speed}, t={t}, r={radius})\n" f"write_output_integer_register(0,1)\n" ) - self.send_command(command, timeout=t + 10, suppress_output=True, raw_response=False) + self.send_command(command, timeout=t + 10, suppress_output=True) if radius == 0.0: timeout = (t + 15.0) if t > 0 else 120.0 - self.rtde.wait_until_stopped(timeout=timeout) + self._rtde.wait_until_stopped(timeout=timeout) def move_cartesian( self, @@ -135,7 +141,7 @@ def move_cartesian( self.send_command(command, suppress_output=True) if radius == 0.0: timeout = (time + 15.0) if time > 0 else 120.0 - self.rtde.wait_until_stopped(timeout=timeout) + self._rtde.wait_until_stopped(timeout=timeout) def stop_motion(self) -> None: deceleration = 2.0 # rad/s^2 @@ -143,73 +149,73 @@ def stop_motion(self) -> None: def get_joint_positions(self) -> list[float]: """Return actual joint positions in radians.""" - angles = self.rtde.joint_angles() + angles = self._rtde.joint_angles() logger.receive(f"Received response: {angles}") return angles def get_joint_velocities(self) -> list[float]: """Return actual joint velocities in rad/s.""" - vels = self.rtde.joint_velocities() + vels = self._rtde.joint_velocities() logger.receive(f"Received response: {vels}") return vels def get_joint_currents(self) -> list[float]: """Return actual joint currents in Amperes.""" - currents = self.rtde.joint_currents() + currents = self._rtde.joint_currents() logger.receive(f"Received response: {currents}") return currents def get_joint_torques(self) -> list[float]: """Return joint torques in Nm. Requires controller >= 5.23.0.0.""" - torques = self.rtde.joint_torques() + torques = self._rtde.joint_torques() logger.receive(f"Received response: {torques}") return torques def get_cartesian_position(self) -> list[float]: """Return actual TCP pose [x, y, z, rx, ry, rz] in metres and radians.""" - pose = self.rtde.tcp_pose() + pose = self._rtde.tcp_pose() logger.receive(f"Received response: {pose}") return pose def get_tcp_speed(self) -> list[float]: """Return actual TCP speed [vx, vy, vz, wx, wy, wz] in m/s and rad/s.""" - speed = self.rtde.tcp_speed() + speed = self._rtde.tcp_speed() logger.receive(f"Received response: {speed}") return speed def get_tcp_forces(self) -> list[float]: """Return TCP force [Fx, Fy, Fz, Tx, Ty, Tz] in Newton and Newton-metres.""" - forces = self.rtde.tcp_force() + forces = self._rtde.tcp_force() logger.receive(f"Received response: {forces}") return forces def get_target_joint_positions(self) -> list[float]: """Return target joint positions in radians.""" - target = self.rtde.target_joint_positions() + target = self._rtde.target_joint_positions() logger.receive(f"Received response: {target}") return target def get_target_joint_velocities(self) -> list[float]: """Return target joint velocities in rad/s.""" - target = self.rtde.target_joint_velocities() + target = self._rtde.target_joint_velocities() logger.receive(f"Received response: {target}") return target def get_target_tcp_pose(self) -> list[float]: """Return target TCP pose [x, y, z, rx, ry, rz] in metres and radians.""" - target = self.rtde.target_tcp_pose() + target = self._rtde.target_tcp_pose() logger.receive(f"Received response: {target}") return target def get_target_tcp_speed(self) -> list[float]: """Return target TCP speed [vx, vy, vz, wx, wy, wz] in m/s and rad/s.""" - target = self.rtde.target_tcp_speed() + target = self._rtde.target_tcp_speed() logger.receive(f"Received response: {target}") return target def get_robot_state(self) -> dict[str, bool]: """Return robot and safety status bit fields.""" - status = self.rtde.robot_status() + status = self._rtde.robot_status() key_out = [ "Power On", "Program Running", @@ -223,79 +229,79 @@ def get_robot_state(self) -> dict[str, bool]: def get_robot_mode(self) -> RobotMode: """Return robot mode as a RobotMode enum.""" - mode = self.rtde.robot_mode() + mode = self._rtde.robot_mode() logger.receive(f"Received response: {mode}") return mode def get_safety_mode(self) -> SafetyMode: """Return safety mode as a SafetyMode enum.""" - mode = self.rtde.safety_mode() + mode = self._rtde.safety_mode() logger.receive(f"Received response: {mode}") return mode def get_runtime_state(self) -> RuntimeState: """Return runtime state as a RuntimeState enum.""" - state = self.rtde.runtime_state() + state = self._rtde.runtime_state() logger.receive(f"Received response: {state}") return state def get_speed_scaling(self) -> float: """Return current speed scaling factor in [0, 1].""" - scaling = self.rtde.speed_scaling() + scaling = self._rtde.speed_scaling() logger.receive(f"Received response: {scaling}") return scaling def get_payload(self) -> dict: """Return payload mass (kg) and centre of gravity (m).""" - payload = self.rtde.payload() + payload = self._rtde.payload() logger.receive(f"Received response: {payload}") return payload def is_moving(self) -> bool: """Return True if the robot is currently in motion.""" - return self.rtde.is_moving() + return self._rtde.is_moving() def get_analog_inputs(self) -> dict[str, float]: """Return standard and tool analog input values.""" - inputs = self.rtde.analog_inputs() + inputs = self._rtde.analog_inputs() logger.receive(f"Received response: {inputs}") return inputs def get_analog_outputs(self) -> dict[str, float]: """Return standard analog output values.""" - outputs = self.rtde.analog_outputs() + outputs = self._rtde.analog_outputs() logger.receive(f"Received response: {outputs}") return outputs def get_digital_inputs(self) -> dict[str, bool]: """Return digital input states (standard, configurable, tool).""" - inputs = self.rtde.digital_inputs() + inputs = self._rtde.digital_inputs() logger.receive(f"Received response: {inputs}") return inputs def get_digital_outputs(self) -> dict[str, bool]: """Return digital output states (standard, configurable, tool).""" - outputs = self.rtde.digital_outputs() + outputs = self._rtde.digital_outputs() logger.receive(f"Received response: {outputs}") return outputs def get_tool_io(self) -> dict: """Return tool I/O state (analog inputs, voltage, current, temperature).""" - tool = self.rtde.tool_io() + tool = self._rtde.tool_io() logger.receive(f"Received response: {tool}") return tool def set_speed_slider(self, fraction: float) -> None: """Set speed override slider fraction in [0.0, 1.0].""" logger.send(f"Setting speed slider: {fraction}") - self.rtde.set_speed_slider(fraction) + self._rtde.set_speed_slider(fraction) def set_digital_output(self, pin: int, value: bool) -> None: """Set a standard digital output pin (0-7) high or low.""" logger.send(f"Setting digital output DO{pin} = {value}") - self.rtde.set_digital_output(pin, value) + self._rtde.set_digital_output(pin, value) def set_analog_output(self, channel: int, value: float) -> None: """Set standard analog output voltage on channel 0 or 1.""" logger.send(f"Setting analog output AO{channel} = {value}V") - self.rtde.set_analog_output(channel, value) + self._rtde.set_analog_output(channel, value) diff --git a/armctl/zaber/zaber.py b/armctl/zaber/zaber.py index 12acf66..9c8ddb2 100644 --- a/armctl/zaber/zaber.py +++ b/armctl/zaber/zaber.py @@ -119,7 +119,7 @@ def move_joints(self, pos: list[float]) -> None: Args: pos: List of positions in meters (one per axis) """ - cc.move_joints(pos) + cc.move_joints(self, pos) try: if len(pos) != len(self.axes): @@ -164,7 +164,7 @@ def move_cartesian(self, pose: list[float]) -> None: Args: pose: List of positions in meters [x, y, z, ...] or applicable axes """ - cc.move_cartesian(pose) + cc.move_cartesian(self, pose) # For linear systems, cartesian movement is same as joint movement self.move_joints(pose[: len(self.axes)]) diff --git a/tests/test_drivers.py b/tests/test_drivers.py index 1f52333..1b0c23c 100644 --- a/tests/test_drivers.py +++ b/tests/test_drivers.py @@ -4,7 +4,8 @@ import pytest -from armctl import Jaka, UniversalRobots, Vention +from armctl import ElephantRobotics, Jaka, Pro600, UniversalRobots, Vention, Zaber +from armctl.universal_robots import OnRobot HOME = [0.0, -1.57, 1.57, 0.0, 0.0, 0.0] @@ -151,3 +152,88 @@ def test_vention_rejects_bad_axis_position(vention, reply): vention.send_command.return_value = reply with pytest.raises(RuntimeError): vention.get_joint_positions(axis=1) + + +@pytest.fixture +def zaber(): + pytest.importorskip("zaber_motion") + robot = Zaber("10.0.0.1", num_axes=2) + robot.axes = [MagicMock(), MagicMock()] + return robot + + +def test_zaber_move_joints_moves_each_axis(zaber): + zaber.move_joints([0.1, 0.2]) + for axis, target in zip(zaber.axes, [0.1, 0.2]): + axis.move_absolute.assert_called_once_with(target, unit=zaber.units) + axis.wait_until_idle.assert_called_once() + + +def test_zaber_rejects_out_of_range(zaber): + with pytest.raises(ValueError): + zaber.move_joints([0.1, 0.5]) + zaber.axes[0].move_absolute.assert_not_called() + + +def test_ur_telemetry_requires_connection(): + with pytest.raises(ConnectionError, match="not connected"): + UniversalRobots("10.0.0.1").get_joint_positions() + + +@pytest.fixture +def elephant(monkeypatch): + monkeypatch.setattr("armctl.elephant_robotics.elephant_robotics.time.sleep", lambda _: None) + robot = ElephantRobotics("10.0.0.1", 5001) + robot.send_command = MagicMock() + return robot + + +def test_elephant_move_joints_sends_integer_degrees_and_waits(elephant): + target = [math.pi / 2, 0.0, 0.0, 0.0, 0.0, 0.0] + command = "set_angles(90.0,0.0,0.0,0.0,0.0,0.0,500)" + elephant.send_command.side_effect = [ + f"{command}:[ok]", + "get_angles:[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]", + "get_angles:[90.0, 0.0, 0.0, 0.0, 0.0, 0.0]", + ] + elephant.move_joints(target) + assert elephant.send_command.call_args_list[0].args[0] == command + assert elephant.send_command.call_count == 3 + + +def test_elephant_get_cartesian_position_returns_meters_and_radians(elephant): + elephant.send_command.return_value = "get_coords:[100.0, 200.0, 300.0, 90.0, 0.0, 180.0]" + assert elephant.get_cartesian_position() == pytest.approx( + [0.1, 0.2, 0.3, math.pi / 2, 0, math.pi] + ) + + +def test_elephant_move_cartesian_sends_mm_and_degrees(elephant): + elephant.send_command.side_effect = [ + "set_coords:[ok]", + "get_coords:[100.0, 200.0, 300.0, 90.0, 0.0, 180.0]", + ] + elephant.move_cartesian([0.1, 0.2, 0.3, math.pi / 2, 0.0, math.pi]) + assert elephant.send_command.call_args_list[0].args[0] == ( + "set_coords(100.0,200.0,300.0,90.0,0.0,180.0,500)" + ) + + +def test_pro600_home_speed_within_limits(monkeypatch): + monkeypatch.setattr("armctl.elephant_robotics.elephant_robotics.time.sleep", lambda _: None) + robot = Pro600() + robot.send_command = MagicMock( + side_effect=[ + "set_angles(0.0,-90.0,90.0,-90.0,-90.0,0.0,750):[ok]", + "get_angles:[0.0, -90.0, 90.0, -90.0, -90.0, 0.0]", + ] + ) + robot.home() + assert robot.send_command.call_count == 2 + + +@pytest.mark.parametrize("reply, closed", [("1\n", True), ("0\n", False)]) +def test_onrobot_status_parses_text_reply(reply, closed): + gripper = OnRobot.__new__(OnRobot) + gripper.send_command = MagicMock(return_value=reply) + assert gripper.get_status() is closed