432 thread to check the send goal positions is taking too long

src/reachy2_sdk/orbita/orbita.py

@@ -235,11 +235,17 @@ def _set_outgoing_goal_position(self, axis_name: str, goal_position: float) -> N
         self._outgoing_goal_positions[axis] = goal_position
 
     @abstractmethod
-    def send_goal_positions(self) -> None:
+    def send_goal_positions(self, check_positions: bool = True) -> None:
         """Send the goal positions to the actuator.
 
         This method is abstract and should be implemented in derived classes to
         send the specified goal positions to the actuator's joints.
+
+        Args:
+            check_positions: A boolean value indicating whether to check the positions of the joints
+                after sending the goal positions. If `True`, a background thread is started to monitor
+                the joint positions relative to their last goal positions.
+                Default is `True`.
         """
         pass
 
@@ -274,8 +280,8 @@ def _check_goal_positions(self) -> None:
             # precision is low we are looking for unreachable positions
             if not np.isclose(orbitajoint.present_position, orbitajoint.goal_position, atol=1):
                 self._logger.warning(
-                    f"required goal position for {self._name}.{joint} is unreachable."
-                    f" current position is ({orbitajoint.present_position}"
+                    f"Required goal position ({round(orbitajoint.goal_position, 2)}) for {self._name}.{joint} is unreachable."
+                    f"\nCurrent position is ({round(orbitajoint.present_position, 2)})."
                 )
 
     def _update_with(self, new_state: Orbita2dState | Orbita3dState) -> None:

src/reachy2_sdk/orbita/orbita2d.py

@@ -173,10 +173,14 @@ def __setattr__(self, __name: str, __value: Any) -> None:
                 raise AttributeError(f"can't set attribute '{__name}'")
         super().__setattr__(__name, __value)
 
-    def send_goal_positions(self) -> None:
+    def send_goal_positions(self, check_positions: bool = True) -> None:
         """Send goal positions to the actuator's joints.
 
         If goal positions have been specified for any joint of this actuator, sends them to the actuator.
+
+        Args :
+            check_positions: A boolean indicating whether to check the positions after sending the command.
+                Defaults to True.
         """
         if self._outgoing_goal_positions:
             req_pos = {}
@@ -195,7 +199,8 @@ def send_goal_positions(self) -> None:
             )
             self._outgoing_goal_positions = {}
             self._stub.SendCommand(command)
-            self._post_send_goal_positions()
+            if check_positions:
+                self._post_send_goal_positions()
 
     def set_speed_limits(self, speed_limit: float | int) -> None:
         """Set the speed limit as a percentage of the maximum speed for all motors of the actuator.

src/reachy2_sdk/orbita/orbita3d.py

@@ -153,10 +153,13 @@ def yaw(self) -> OrbitaJoint:
         """Get the yaw joint of the actuator."""
         return self._yaw
 
-    def send_goal_positions(self) -> None:
+    def send_goal_positions(self, check_positions: bool = True) -> None:
         """Send goal positions to the actuator's joints.
 
         If goal positions have been specified for any joint of this actuator, sends them to the actuator.
+
+        Args:
+            check_positions: A boolean indicating whether to check the positions after sending the command.
         """
         if self._outgoing_goal_positions:
             req_pos = {}
@@ -175,7 +178,8 @@ def send_goal_positions(self) -> None:
             )
             self._outgoing_goal_positions = {}
             self._stub.SendCommand(command)
-            self._post_send_goal_positions()
+            if check_positions:
+                self._post_send_goal_positions()
 
     def set_speed_limits(self, speed_limit: float | int) -> None:
         """Set the speed limit as a percentage of the maximum speed for all motors of the actuator.

src/reachy2_sdk/parts/arm.py

@@ -1104,18 +1104,22 @@ def _send_elliptical_interpolation(
             self._stub.SendArmCartesianGoal(request)
             time.sleep(time_step)
 
-    def send_goal_positions(self) -> None:
+    def send_goal_positions(self, check_positions: bool = True) -> None:
         """Send goal positions to the gripper and actuators if the parts are on.
 
         The function checks if the gripper and actuators are active before sending the goal positions.
+
+        Args :
+            check_positions: A boolean indicating whether to check the positions after sending the command.
+                Defaults to True.
         """
         if self._gripper is not None:
-            self._gripper.send_goal_positions()
+            self._gripper.send_goal_positions(check_positions)
         if self.is_off():
             self._logger.warning(f"{self._part_id.name} is off. Command not sent.")
             return
         for actuator in self._actuators.values():
-            actuator.send_goal_positions()
+            actuator.send_goal_positions(check_positions)
 
     def _update_with(self, new_state: ArmState) -> None:
         """Update the arm with a newly received (partial) state from the gRPC server.

src/reachy2_sdk/parts/hand.py

@@ -230,11 +230,15 @@ def set_opening(self, percentage: float) -> None:
         )
         self._is_moving = True
 
-    def send_goal_positions(self) -> None:
+    def send_goal_positions(self, check_positions: bool = True) -> None:
         """Send the goal position to the hand actuator.
 
         If any goal position has been specified to the gripper, sends them to the robot.
         If the hand is off, the command is not sent.
+
+        Args :
+            check_positions: A boolean indicating whether to check the positions after sending the command.
+                Defaults to True.
         """
         if self.is_off():
             self._logger.warning(f"{self._part_id.name} is off. Command not sent.")

src/reachy2_sdk/parts/head.py

@@ -351,16 +351,20 @@ def goto_posture(
             self._logger.warning("Head is off. No command sent.")
         return GoToId(id=-1)
 
-    def send_goal_positions(self) -> None:
+    def send_goal_positions(self, check_positions: bool = True) -> None:
         """Send goal positions to the head's joints.
 
         If goal positions have been specified for any joint of the head, sends them to the robot.
+
+        Args :
+            check_positions: A boolean indicating whether to check the positions after sending the command.
+                Defaults to True.
         """
         if self.is_off():
             self._logger.warning(f"{self._part_id.name} is off. Command not sent.")
             return
         for actuator in self._actuators.values():
-            actuator.send_goal_positions()
+            actuator.send_goal_positions(check_positions)
 
     def _update_with(self, new_state: HeadState) -> None:
         """Update the head with a newly received (partial) state from the gRPC server.

src/reachy2_sdk/parts/joints_based_part.py

@@ -69,10 +69,14 @@ def get_current_positions(self) -> List[float]:
         pass
 
     @abstractmethod
-    def send_goal_positions(self) -> None:
+    def send_goal_positions(self, check_positions: bool = True) -> None:
         """Send goal positions to the part's joints.
 
         If goal positions have been specified for any joint of the part, sends them to the robot.
+
+        Args :
+            check_positions: A boolean indicating whether to check the positions after sending the command.
+                Defaults to True.
         """
         pass
 

src/reachy2_sdk/reachy_sdk.py

@@ -777,15 +777,19 @@ def cancel_all_goto(self) -> GoToAck:
         response = self._goto_stub.CancelAllGoTo(Empty())
         return response
 
-    def send_goal_positions(self) -> None:
+    def send_goal_positions(self, check_positions: bool = True) -> None:
         """Send the goal positions to the robot.
 
         If goal positions have been specified for any joint of the robot, sends them to the robot.
+
+        Args :
+            check_positions: A boolean indicating whether to check the positions after sending the command.
+                Defaults to True.
         """
         if not self.info:
             self._logger.warning("Reachy is not connected!")
             return
 
         for part in self.info._enabled_parts.values():
             if issubclass(type(part), JointsBasedPart):
-                part.send_goal_positions()
+                part.send_goal_positions(check_positions)