Merge pull request #29 from alberto-abarzua/28-firmware-+-controller-…

…add-setting-to-invert-direction-and-none-endstop

feat: add invert direction and none endstop

backend/Dockerfile

@@ -2,15 +2,18 @@ FROM python:3.11-slim-buster
 
 WORKDIR /app
 
-RUN apt-get update && apt-get install -y build-essential
+# hadolint ignore=DL3008
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    build-essential && \
+    rm -rf /var/lib/apt/lists/*
 
 ENV PYTHONDONTWRITEBYTECODE 1
 ENV PYTHONUNBUFFERED 1
 
 ENV PYTHONPATH=/app/src
 
 
-RUN pip install pdm==2.10
+RUN pip install --no-cache-dir pdm==2.10
 
 COPY ./pyproject.toml ./pyproject.toml
 COPY ./pdm.lock ./pdm.lock

backend/src/config/main_arm.toml

@@ -7,6 +7,7 @@ title = "Robot arm configuration"
     min_angle_rad = -7.0
     conversion_rate_axis_joint = 1.0 
     homing_offset_rad = 0.0 # Offset from homing position
+    dir_inverted = 1 # 1 or -1 
     # Settings can be overwritten for each joint
 
 [arm_parameters]
@@ -41,6 +42,7 @@ title = "Robot arm configuration"
     conversion_rate_axis_joint = 9.3 # Gear ratio
     homing_offset_rad = 0 # Offset from home position
     speed_rad_per_s = 0.5
+    dir_inverted = -1
     [joints.driver]
         type = "stepper"
         dir_pin = 14
@@ -65,7 +67,7 @@ title = "Robot arm configuration"
         dir_pin = 5
         step_pin = 4
     [joints.endstop]
-        type = "hall"
+        type = "none"
         pin = 25
 
 [[joints]]

controller/Dockerfile

@@ -5,7 +5,7 @@ WORKDIR /app
 ENV PYTHONDONTWRITEBYTECODE 1
 ENV PYTHONUNBUFFERED 1
 
-RUN pip install pdm==2.10
+RUN pip install --no-cache-dir pdm==2.10
 
 ENV PYTHONPATH "${PYTHONPATH}:/app/src"
 

controller/src/robot_arm_controller/controller.py

@@ -17,6 +17,7 @@
     ControllerServer,
     WebsocketServer,
 )
+from robot_arm_controller.utils.algebra import allclose
 from robot_arm_controller.utils.fifo_lock import FIFOLock
 from robot_arm_controller.utils.messages import Message, MessageOp
 from robot_arm_controller.utils.prints import console
@@ -28,6 +29,7 @@ class Settings(Enum):
     STEPS_PER_REV_MOTOR_AXIS = 9
     CONVERSION_RATE_AXIS_JOINTS = 13
     HOMING_OFFSET_RADS = 17
+    DIR_INVERTED = 31
 
 
 @dataclasses.dataclass
@@ -94,6 +96,7 @@ def __init__(
                 Settings.STEPS_PER_REV_MOTOR_AXIS: Setting(value=200, code_set=9, code_get=11),
                 Settings.CONVERSION_RATE_AXIS_JOINTS: Setting(value=1, code_set=13, code_get=15),
                 Settings.HOMING_OFFSET_RADS: Setting(value=np.pi / 4, code_set=17, code_get=19),
+                Settings.DIR_INVERTED: Setting(value=0, code_set=31, code_get=33),
             }
             self.joint_settings.append(current_joint_settings)
             self.joint_settings_response_code.append(
@@ -200,6 +203,7 @@ def configure_from_file(self, file: Path, reload: bool = True) -> None:
         default_homing_offset_rads = joint_configuration["homing_offset_rad"]
         default_min_angle_rad = joint_configuration["min_angle_rad"]
         default_max_angle_rad = joint_configuration["max_angle_rad"]
+        default_dir_inverted = joint_configuration["dir_inverted"]
 
         for i, joint in enumerate(joints):
             if "speed_rad_per_s" not in joint.keys():
@@ -223,13 +227,17 @@ def configure_from_file(self, file: Path, reload: bool = True) -> None:
             if "max_angle_rad" not in joint.keys():
                 joint["max_angle_rad"] = default_max_angle_rad
 
+            if "dir_inverted" not in joint.keys():
+                joint["dir_inverted"] = default_dir_inverted
+
             speed = joint["speed_rad_per_s"]
             homing_direction = joint["homing_direction"]
             steps_per_rev_motor_axis = joint["steps_per_rev_motor_axis"]
             conversion_rate_axis_joint = joint["conversion_rate_axis_joint"]
             homing_offset_rads = joint["homing_offset_rad"]
             min_angle_rad = joint["min_angle_rad"]
             max_angle_rad = joint["max_angle_rad"]
+            dir_inverted = joint["dir_inverted"]
 
             driver = joint["driver"]
             if driver["type"] == "stepper":
@@ -246,12 +254,15 @@ def configure_from_file(self, file: Path, reload: bool = True) -> None:
                 self.set_joint_endstop_hall(i, pin)
             elif endstop["type"] == "dummy":
                 self.set_joint_endstop_dummy(i)
+            elif endstop["type"] == "none":
+                self.set_joint_endstop_none(i)
 
             self.set_setting_joint(Settings.SPEED_RAD_PER_S, speed, i)
             self.set_setting_joint(Settings.HOMING_DIRECTION, homing_direction, i)
             self.set_setting_joint(Settings.STEPS_PER_REV_MOTOR_AXIS, steps_per_rev_motor_axis, i)
             self.set_setting_joint(Settings.CONVERSION_RATE_AXIS_JOINTS, conversion_rate_axis_joint, i)
             self.set_setting_joint(Settings.HOMING_OFFSET_RADS, homing_offset_rads, i)
+            self.set_setting_joint(Settings.DIR_INVERTED, dir_inverted, i)
             self.arm_params.joints[i].set_bounds(min_angle_rad, max_angle_rad)
 
         if reload:
@@ -406,7 +417,7 @@ def set_tool_value(self, angle: float) -> None:
             console.log(f"Setting tool value to: {angle}", style="set_tool")
 
     def wait_until_angles_at_target(self, target_angles: List[float], epsilon: float = 0.01) -> None:
-        while not np.allclose(self.current_angles, target_angles, atol=epsilon) and not self.stop_event.is_set():
+        while not allclose(self.current_angles, target_angles, atol=epsilon) and not self.stop_event.is_set():
             time.sleep(0.2)
 
     def wait_done_moving(self) -> None:
@@ -451,7 +462,7 @@ def set_setting_joint(self, setting_key: Settings, value: float, joint_idx: int)
         setting.last_updated = -1
 
         if self.print_status:
-            console.log(f"Setting {setting_key} to {value} for joint {joint_idx}", style="set_settings")
+            console.log(f"Setting {setting_key}:{code} to {value} for joint {joint_idx}", style="set_settings")
 
     def set_setting_joints(self, setting_key: Settings, value: float) -> None:
         for joint_idx in range(self.num_joints):
@@ -498,6 +509,12 @@ def set_joint_endstop_dummy(self, joint_idx: int) -> None:
         if self.print_status:
             console.log(f"Setting endstop dummy for joint {joint_idx}", style="set_settings")
 
+    def set_joint_endstop_none(self, joint_idx: int) -> None:
+        message = Message(MessageOp.CONFIG, 29, [float(joint_idx)])
+        self.controller_server.send_message(message, mutex=True)
+        if self.print_status:
+            console.log(f"Setting endstop none for joint {joint_idx}", style="set_settings")
+
 
 class SingletonArmController:
     _instance: Optional[ArmController] = None

controller/src/robot_arm_controller/tests/test_controller.py

@@ -7,6 +7,7 @@
 
 from robot_arm_controller.control.arm_kinematics import ArmParameters
 from robot_arm_controller.controller import ArmController, Settings
+from robot_arm_controller.utils.algebra import allclose
 from robot_arm_controller.utils.prints import console
 
 
@@ -61,40 +62,40 @@ def test_move_to(self) -> None:
         self.controller.move_joints_to(angles)
         self.controller.wait_done_moving()
 
-        all_close = np.allclose(self.controller.current_angles, angles, atol=epsilon)
+        all_close = allclose(self.controller.current_angles, angles, atol=epsilon)
         self.assertTrue(all_close, msg=f"Expected {angles}, got {self.controller.current_angles}")
 
         angles = [0.2, 0.3, 0.4, 0.5, 0.6, 0.7]
         self.controller.move_joints_to(angles)
         self.controller.wait_done_moving()
 
-        all_close = np.allclose(self.controller.current_angles, angles, atol=epsilon)
+        all_close = allclose(self.controller.current_angles, angles, atol=epsilon)
 
         self.assertTrue(all_close, msg=f"Expected {angles}, got {self.controller.current_angles}")
 
         angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         self.controller.move_joints_to(angles)
         self.controller.wait_done_moving()
 
-        all_close = np.allclose(self.controller.current_angles, angles, atol=epsilon)
+        all_close = allclose(self.controller.current_angles, angles, atol=epsilon)
         self.assertTrue(all_close, msg=f"Expected {angles}, got {self.controller.current_angles}")
 
         angles = [-0.1, -0.2, -0.3, -0.4, -0.5, -0.6]
         self.controller.move_joints_to(angles)
         self.controller.wait_done_moving()
 
-        all_close = np.allclose(self.controller.current_angles, angles, atol=epsilon)
+        all_close = allclose(self.controller.current_angles, angles, atol=epsilon)
         self.assertTrue(all_close, msg=f"Expected {angles}, got {self.controller.current_angles}")
 
     def test_double_home(self) -> None:
         console.log("Running test: test_double_home", style="bold yellow")
         self.controller.wait_done_moving()
         self.controller.home()
-        self.assertTrue(np.allclose(self.controller.current_angles, [0, 0, 0, 0, 0, 0], atol=0.1))
+        self.assertTrue(allclose(self.controller.current_angles, [0, 0, 0, 0, 0, 0], atol=0.1))
         self.assertTrue(self.controller.is_homed)
         self.controller.home()
         self.assertTrue(self.controller.is_homed)
-        self.assertTrue(np.allclose(self.controller.current_angles, [0, 0, 0, 0, 0, 0], atol=0.1))
+        self.assertTrue(allclose(self.controller.current_angles, [0, 0, 0, 0, 0, 0], atol=0.1))
 
     def get_and_set_joint_settings(
         self, setting_key: Settings, init_value: float, change_per_iter: bool = True
@@ -103,34 +104,44 @@ def get_and_set_joint_settings(
 
         # homing_direction
         correct = True
+        setting = None
+        value = None
         for i in range(self.controller.num_joints):
+            value = init_value
             if change_per_iter:
                 value = init_value + i
-            else:
-                value = init_value
             self.controller.set_setting_joint(setting_key, value, i)
             setting = self.controller.get_setting_joint(setting_key, i)
             correct = correct and (abs(setting - value) < self.EPSILON)
+            if not correct:
+                break
+
         return correct, f"Expected {value}, got {setting}"
 
     def test_joint_settings(self) -> None:
         console.log("Running test: test_joint_settings", style="bold yellow")
+
         correct, msg = self.get_and_set_joint_settings(Settings.HOMING_DIRECTION, -1, False)
+        self.assertTrue(correct, msg)
         correct, msg = self.get_and_set_joint_settings(Settings.HOMING_DIRECTION, 1, False)
 
         self.assertTrue(correct, msg)
         correct, msg = self.get_and_set_joint_settings(Settings.SPEED_RAD_PER_S, 1.2)
 
         self.assertTrue(correct, msg)
         correct, msg = self.get_and_set_joint_settings(Settings.STEPS_PER_REV_MOTOR_AXIS, 220)
+        self.assertTrue(correct, msg)
         correct, msg = self.get_and_set_joint_settings(Settings.STEPS_PER_REV_MOTOR_AXIS, 5000, False)
 
         self.assertTrue(correct, msg)
         correct, msg = self.get_and_set_joint_settings(Settings.CONVERSION_RATE_AXIS_JOINTS, 1.7)
+        self.assertTrue(correct, msg)
         correct, msg = self.get_and_set_joint_settings(Settings.CONVERSION_RATE_AXIS_JOINTS, 1, False)
 
         self.assertTrue(correct, msg)
         correct, msg = self.get_and_set_joint_settings(Settings.HOMING_OFFSET_RADS, np.pi / 7)
+
+        self.assertTrue(correct, msg)
         correct, msg = self.get_and_set_joint_settings(Settings.HOMING_OFFSET_RADS, np.pi / 4, False)
 
         self.assertTrue(correct, msg)
@@ -140,15 +151,15 @@ def test_speed_settings(self) -> None:
         self.controller.wait_done_moving()
         self.controller.set_setting_joints(Settings.SPEED_RAD_PER_S, 1)
         speeds = self.controller.get_setting_joints(Settings.SPEED_RAD_PER_S)
-        self.assertTrue(np.allclose(speeds, [1, 1, 1, 1, 1, 1], atol=0.1))
+        self.assertTrue(allclose(speeds, [1, 1, 1, 1, 1, 1], atol=0.1))
         self.controller.move_joints_to([0, 0, 0, 0, 0, 0])
         self.controller.wait_done_moving()
         start_time = time.time()
         self.controller.move_joints_to([1, 1, 1, 1, 1, 1])
         self.controller.wait_done_moving()
         end_time = time.time()
         speeds = self.controller.get_setting_joints(Settings.SPEED_RAD_PER_S)
-        self.assertTrue(np.allclose(speeds, [1, 1, 1, 1, 1, 1], atol=0.1))
+        self.assertTrue(allclose(speeds, [1, 1, 1, 1, 1, 1], atol=0.1))
         self.assertTrue(end_time - start_time < 1.4, msg=f"Expected 1s, got {end_time - start_time}")
 
         self.controller.move_joints_to([0, 0, 0, 0, 0, 0])
@@ -165,30 +176,30 @@ def test_set_tool_value(self) -> None:
         self.controller.set_tool_value(0)
         self.controller.wait_done_moving()
 
-        all_close = np.allclose(self.controller.tool_value, 0, atol=0.15)
+        all_close = allclose(self.controller.tool_value, 0, atol=0.15)
         self.assertTrue(all_close, msg=f"Expected {0} got {self.controller.tool_value}")
 
         self.controller.set_tool_value(1)
         self.controller.wait_done_moving()
 
-        all_close = np.allclose(self.controller.tool_value, 1, atol=0.15)
+        all_close = allclose(self.controller.tool_value, 1, atol=0.15)
         self.assertTrue(all_close, msg=f"Expected {1} got {self.controller.tool_value}")
 
         self.controller.set_tool_value(-1)
         self.controller.wait_done_moving()
 
-        all_close = np.allclose(self.controller.tool_value, -1, atol=0.15)
+        all_close = allclose(self.controller.tool_value, -1, atol=0.15)
         self.assertTrue(all_close, msg=f"Expected {-1} got {self.controller.tool_value}")
 
         self.controller.set_tool_value(0.5)
         self.controller.wait_done_moving()
 
-        all_close = np.allclose(self.controller.tool_value, 0.5, atol=0.15)
+        all_close = allclose(self.controller.tool_value, 0.5, atol=0.15)
         self.assertTrue(all_close, msg=f"Expected {0.5} got {self.controller.tool_value}")
 
         self.controller.set_tool_value(-0.5)
         self.controller.wait_done_moving()
-        all_close = np.allclose(self.controller.tool_value, -0.5, atol=0.15)
+        all_close = allclose(self.controller.tool_value, -0.5, atol=0.15)
         self.assertTrue(all_close, msg=f"Expected {-0.5} got {self.controller.tool_value}")
 
     def test_speed_with_modified_settings(self) -> None:
@@ -211,27 +222,27 @@ def test_speed_with_modified_settings(self) -> None:
 
         self.controller.set_setting_joints(Settings.SPEED_RAD_PER_S, 1)
         speeds = self.controller.get_setting_joints(Settings.SPEED_RAD_PER_S)
-        self.assertTrue(np.allclose(speeds, [1, 1, 1, 1, 1, 1], atol=0.1))
+        self.assertTrue(allclose(speeds, [1, 1, 1, 1, 1, 1], atol=0.1))
 
         self.controller.wait_done_moving()
 
-        self.assertTrue(np.allclose(self.controller.current_angles, [0, 0, 0, 0, 0, 0], atol=0.1))
+        self.assertTrue(allclose(self.controller.current_angles, [0, 0, 0, 0, 0, 0], atol=0.1))
         start_time = time.time()
         self.controller.move_joints_to([1, 1, 1, 1, 1, 1])
         self.controller.wait_done_moving()
-        self.assertTrue(np.allclose(self.controller.current_angles, [1, 1, 1, 1, 1, 1], atol=0.1))
+        self.assertTrue(allclose(self.controller.current_angles, [1, 1, 1, 1, 1, 1], atol=0.1))
         end_time = time.time()
         self.assertTrue(end_time - start_time < 1.4, msg=f"Expected 1s, got {end_time - start_time}")
 
         speeds = self.controller.get_setting_joints(Settings.SPEED_RAD_PER_S)
-        self.assertTrue(np.allclose(speeds, [1, 1, 1, 1, 1, 1], atol=0.1))
+        self.assertTrue(allclose(speeds, [1, 1, 1, 1, 1, 1], atol=0.1))
 
     def test_move_queue_size(self) -> None:
         self.controller.wait_done_moving()
         self.controller.move_joints_to([0, 0, 0, 0, 0, 0])
         time.sleep(1)
         self.controller.wait_done_moving()
-        self.assertTrue(np.allclose(self.controller.current_angles, [0, 0, 0, 0, 0, 0], atol=0.1))
+        self.assertTrue(allclose(self.controller.current_angles, [0, 0, 0, 0, 0, 0], atol=0.1))
         self.controller.set_setting_joints(Settings.SPEED_RAD_PER_S, 0.2)
         self.controller.move_joints_to([-1, -1, -1, -1, -1, -1])
         time.sleep(1)

controller/src/robot_arm_controller/tests/test_kinematics.py

@@ -6,6 +6,7 @@
 from robot_arm_controller.control.arm_kinematics import ArmParameters, ArmPose
 from robot_arm_controller.controller import ArmController
 from robot_arm_controller.utils.algebra import (
+    allclose,
     create_rotation_matrix_from_euler_angles,
     degree2rad,
     extract_euler_angles,
@@ -22,16 +23,16 @@ def helper_angle_to_pose(self, angles: List[float], expected_pose: ArmPose) -> N
         current_pose = self.controller.kinematics.angles_to_pose(angles)
         curren_pose_list = current_pose.as_list
         expected_pose_list = expected_pose.as_list
-        all_close = np.allclose(curren_pose_list, expected_pose_list, rtol=self.EPSILON)
+        all_close = allclose(curren_pose_list, expected_pose_list, rtol=self.EPSILON)
         self.assertTrue(all_close, f"expected: {expected_pose_list} actual: {curren_pose_list}")
 
     def helper_pose_to_angles(self, expected_angles: List[float], pose: ArmPose) -> None:
         angles = self.controller.kinematics.pose_to_angles(pose, self.controller.current_angles)
         expected_angles = [degree2rad(angle) for angle in expected_angles]
-        # convert to np.array of float
-        all_close = np.allclose(
-            np.array(angles, dtype=np.float64), np.array(expected_angles, dtype=np.float64), rtol=self.EPSILON
-        )
+        self.assertIsNotNone(angles)
+        if angles is None:
+            return
+        all_close = allclose(angles, expected_angles, rtol=self.EPSILON)
         self.assertTrue(all_close, f"expected: {expected_angles} actual: {angles}")
 
     @classmethod
@@ -86,15 +87,20 @@ def test_angle_to_pose_to_angle(self) -> None:
         angles = [np.pi / 2 for _ in range(6)]
         pose = self.controller.kinematics.angles_to_pose(angles)
         angles2 = self.controller.kinematics.pose_to_angles(pose, self.controller.current_angles)
-        all_close = np.allclose(angles, np.array(angles2, dtype=np.float64), rtol=self.EPSILON)
+
+        if angles2 is None:
+            self.fail("angles2 is None")
+        all_close = allclose(angles, angles2, rtol=self.EPSILON)
         self.assertTrue(all_close, f"expected: {angles} actual: {angles2}")
 
     @disable_console
     def test_rotation_matrixs(self) -> None:
         R1 = create_rotation_matrix_from_euler_angles(90, 100, 20)
 
         roll, pitch, yaw = extract_euler_angles(R1)
+
         R2 = create_rotation_matrix_from_euler_angles(roll, pitch, yaw)
+
         all_close = np.allclose(R1, R2, rtol=self.EPSILON)
         self.assertTrue(all_close, f"expected: {R1} actual: {R2}")
 
@@ -113,5 +119,5 @@ def test_stress(self) -> None:
             self.assertIsNotNone(angles)
             if angles is not None:
                 pose2 = self.controller.kinematics.angles_to_pose(angles)
-                all_close = np.allclose(pose.as_list, pose2.as_list, rtol=self.EPSILON)
+                all_close = allclose(pose.as_list, pose2.as_list, rtol=self.EPSILON)
                 self.assertTrue(all_close, f"expected: {pose.as_list} actual: {pose2.as_list}")