chore: minor bugs in controller pos

backend/src/config/main_arm.toml

@@ -48,8 +48,9 @@ title = "Robot arm configuration"
     name = "joint_1"
     homing_direction = -1 # 1 or -1
     conversion_rate_axis_joint = 9.7 # Gear ratio
-    homing_offset_rad = 0 # Offset from home position
-    speed_rad_per_s = 0.4
+    homing_offset_rad = 0.0 # Offset from home position
+
+    speed_rad_per_s = 0.2
     [joints.driver]
         type = "stepper"
         dir_pin = 14
@@ -69,7 +70,7 @@ title = "Robot arm configuration"
     dir_inverted = -1
     conversion_rate_axis_joint = 7.0 
     homing_offset_rad = 0.0 
-    speed_rad_per_s = 0.05
+    speed_rad_per_s = 0.1
     [joints.driver]
         type = "stepper"
         dir_pin = 5
@@ -83,7 +84,7 @@ title = "Robot arm configuration"
     homing_direction = 1 
     conversion_rate_axis_joint = 15.0
     homing_offset_rad = 0.0
-    speed_rad_per_s = 0.2
+    speed_rad_per_s = 0.1
     steps_per_rev_motor_axis = 400 # Microstepping
     [joints.driver]
         type = "stepper"

controller/src/ribot/controller.py

@@ -50,6 +50,8 @@ def __init__(
         self._current_angles: List[float] = [0, 0, 0, 0, 0, 0]
         self.current_angles_lock: FIFOLock = FIFOLock()
 
+        self.target_pose: Optional[ArmPose] = None
+
         self.tool_value: float = 0
         self.move_queue_size: int = 0
         self.is_homed: bool = False
@@ -347,6 +349,7 @@ def move_to(
         self,
         pose: ArmPose,
     ) -> bool:
+        self.target_pose = pose
         target_angles = self.kinematics.pose_to_angles(pose, self.current_angles)
         if target_angles is None:
             console.log("Target pose is not reachable", style="error")
@@ -357,8 +360,11 @@ def move_to_relative(
         self,
         pose: ArmPose,
     ) -> bool:
-        target_pose = self.current_pose + pose
-        return self.move_to(target_pose)
+        if self.target_pose is None:
+            self.target_pose = self.current_pose
+
+        self.target_pose = self.target_pose + pose
+        return self.move_to(self.target_pose)
 
     def home(self, wait: bool = True) -> None:
         if self.print_status:
@@ -387,6 +393,7 @@ def home(self, wait: bool = True) -> None:
     def move_joint_to(self, joint_idx: int, angle: float) -> bool:
         message = Message(MessageOp.MOVE, 9, [joint_idx, angle])
         self.controller_server.send_message(message, mutex=True)
+        self.target_pose = None
         self.move_queue_size += 1
         if self.print_status:
             console.log(f"Moving joint {joint_idx} to angle: {angle}", style="move_joints")
@@ -400,6 +407,7 @@ def home_joint(self, joint_idx: int) -> None:
     def move_joint_to_relative(self, joint_idx: int, angle: float) -> bool:
         message = Message(MessageOp.MOVE, 11, [joint_idx, angle])
         self.controller_server.send_message(message, mutex=True)
+        self.target_pose = None
         self.move_queue_size += 1
         if self.print_status:
             console.log(f"Moving joint {joint_idx} relative angle: {angle}", style="move_joints")
@@ -408,6 +416,7 @@ def move_joint_to_relative(self, joint_idx: int, angle: float) -> bool:
     def move_joints_to_relative(self, angles: List[float]) -> bool:
         message = Message(MessageOp.MOVE, 13, angles)
         self.controller_server.send_message(message, mutex=True)
+        self.target_pose = None
         self.move_queue_size += 1
         if self.print_status:
             console.log(f"Moving joints relative angles: {angles}", style="move_joints")
@@ -559,7 +568,8 @@ def create_instance(
         cls.print_status = print_status
         cls.config_file = config_file
 
-        cls._instance = ArmController(arm_parameters=arm_parameters, websocket_port=websocket_port, server_port=server_port)
+        cls._instance = ArmController(arm_parameters=arm_parameters,
+                                      websocket_port=websocket_port, server_port=server_port)
         cls._instance.print_status = print_status
         if config_file is not None:
             cls._instance.set_config_file(config_file)

controller/src/ribot/utils/algebra.py

@@ -136,15 +136,17 @@ def create_rotation_matrix_from_euler_angles(roll: float, pitch: float, yaw: flo
 
 
 def nearest_by_2pi_ref(angle: float, ref: float) -> float:
-    """Find the nearest angle to 'ref' that is a 2π away from 'angle' in any direction.
+    """Find the nearest angle to 'ref' that is a multiple of 2π away from 'angle' in any direction.
 
     Args:
         angle (float): The starting angle in radians.
         ref (float): The reference angle in radians.
 
     Returns:
-        float: The nearest angle to 'ref' that is a 2π away from 'angle' in any direction.
+        float: The nearest angle to 'ref' that is a multiple of 2π away from 'angle' in any direction.
     """
-    two_pi = 2 * np.pi
-    n = round((ref - angle) / two_pi)
-    return angle + n * two_pi
+    try:
+        n = round((ref - angle) / (2*np.pi))
+        return angle + n * 2*np.pi
+    except:
+        return angle