Merge 1dc8d65 into d075045

Author: FLo-ABB

examples/simple/example_path_correction.py

@@ -0,0 +1,72 @@
+from ABBRobotEGM import EGM
+import numpy as np
+import time
+
+# Ensure the robot is configured correctly by setting up "EGMPathCorr" with "Path" level in the Motion configuration.
+# This can be done in the robot's controller under: Motion configuration -> External Motion Interface Data.
+# MODULE MainModule
+# VAR egmident egmID1;
+# CONST robtarget Target_10:=[[0.771953305,2.548198209,204.864360938],[-0.000000176,-0.000000006,1,-0.000000002],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
+# TASK PERS wobjdata Workobject_1:=[FALSE,TRUE,"",[[525,-125,308],[1,0,0,0]],[[0,0,0],[1,0,0,0]]];
+# PERS tooldata Pen_TCP:=[TRUE,[[110,0,140],[0.707106781,0,0.707106781,0]],[1,[10.7435139,0,140],[1,0,0,0],0,0,0]];
+
+# PROC main()
+#     EGMReset egmID1;
+#     EGMGetId egmID1;
+#     EGMSetupUC ROB_1,egmId1,"EGMPathCorr","UCdevice"\PathCorr\APTR;
+#     EGMActMove EGMid1,Pen_TCP.tframe\SampleRate:=48;
+#     MoveL Target_10,vmax,fine,Pen_TCP\WObj:=Workobject_1;
+#     EGMMoveL egmID1,Offs(Target_10,200,0,0),v10,fine,Pen_TCP\WObj:=Workobject_1;
+#     MoveL Offs(Target_10,0,0,200),vmax,fine,Pen_TCP\WObj:=Workobject_1;
+#     EGMStop egmID1,EGM_STOP_HOLD;
+# ENDPROC
+# ENDMODULE
+
+
+def main() -> None:
+    """
+    Example showing how to apply path corrections during robot movement.
+    This will apply a sinusoidal correction in the Y direction while the robot
+    moves along a straight line in the X direction (using EGMMoveL in RAPID).
+
+    The sinusoidal pattern:
+    - Amplitude: 100mm
+    - Frequency: 2.0 Hz
+
+    This creates a wavy pattern perpendicular to the robot's movement direction. Run the python script before
+    running the RAPID program on the robot.
+    """
+    with EGM() as egm:
+        print("Waiting for initial message from robot...")
+        # Wait for first message from robot to establish connection
+        while True:
+            success, _ = egm.receive_from_robot(timeout=1.0)
+            if success:
+                print("Connected to robot!")
+                break
+        # Parameters for sinusoidal correction
+        amplitude = 100.0    # mm
+        frequency = 2.0      # Hz
+        t_start = time.time()
+        print("Sending Y-axis path corrections...")
+
+        while True:
+            # Always receive from robot first to maintain connection
+            success, _ = egm.receive_from_robot(timeout=0.1)
+            if not success:
+                print("Lost connection to robot")
+                break
+
+            # Calculate Y correction using sine wave
+            t = time.time() - t_start
+            y_correction = amplitude * np.sin(2 * np.pi * frequency * t)
+
+            correction = np.array([0.0, y_correction, 0.0])
+            egm.send_to_robot_path_corr(correction, age=1)
+
+            # Match robot's sensor refresh rate of 48ms
+            time.sleep(0.048)
+
+
+if __name__ == "__main__":
+    main()

src/ABBRobotEGM/egm.py

@@ -466,34 +466,49 @@ def _create_sensor_message_cart(self, pos: np.ndarray, orient: np.ndarray, speed
 
     def send_to_robot_path_corr(self, pos: np.ndarray, age: float = 1) -> bool:
         """
-        Send a path correction command. Returns False if no data has been received from the robot yet. The path
-        correction is a displacement [x,y,z] in millimeters in **path coordinates**. The displacement uses
-        "path coordinates", which relate the direction of movement of the end effector. See `CorrConn` command in
-        *Technical reference manual RAPID Instructions, Functions and Data types* for a detailed description of path
-        coordinates.  The EGM operation must have been started with EGMActMove, and use EGMMoveL and EGMMoveC commands.
+        Send a path correction command to the robot. The path correction is a displacement [x,y,z]
+        in millimeters in path coordinates. Path coordinates relate to the direction of movement
+        of the end effector.
+
+        The EGM operation must have been started with EGMActMove, and use EGMMoveL or EGMMoveC commands.
+        See CorrConn command in Technical reference manual for details about path coordinates.
 
         :param pos: The displacement in path coordinates in millimeters [x,y,z]
+        :param age: Age of the correction in seconds (must be positive). Defaults to 1
         :return: True if successful, False if no data received from robot yet
+        :raises ValueError: If pos is not a 3-element array or age is not positive
         """
-        self.send_sequence_number += 1
-        sensor_message = self._create_sensor_message_path_corr(pos, age)
-        return self._send_message(sensor_message)
+        if not self.egm_addr:
+            return False
 
-    def _create_sensor_message_path_corr(self, pos: np.ndarray, age: float) -> Any:
-        """Create the sensor message with path correction data to be sent to the robot."""
+        # Input validation
+        try:
+            pos = np.asarray(pos, dtype=np.float64).flatten()
+            if pos.size != 3:
+                raise ValueError("pos must be a 3-element array [x,y,z]")
+        except (ValueError, TypeError):
+            raise ValueError("pos must be convertible to a numpy array")
+
+        if age <= 0:
+            raise ValueError("age must be positive")
+
+        # Create path correction message
         sensor_message = egm_pb2.EgmSensorPathCorr()
+
+        # Set header with path correction message type
         header = sensor_message.header
         header.mtype = egm_pb2.EgmHeader.MessageType.Value('MSGTYPE_PATH_CORRECTION')
         header.seqno = self.send_sequence_number
         self.send_sequence_number += 1
 
+        # Set path correction data
         path_corr = sensor_message.pathCorr
-        path_corr.pos.x = pos[0]
-        path_corr.pos.y = pos[1]
-        path_corr.pos.z = pos[2]
-        path_corr.age = age
+        path_corr.pos.x = float(pos[0])
+        path_corr.pos.y = float(pos[1])
+        path_corr.pos.z = float(pos[2])
+        path_corr.age = int(age)  # Protocol expects unsigned integer
 
-        return sensor_message
+        return self._send_message(sensor_message)
 
     def _get_timestamps(self, robot_message: Any) -> Tuple[Optional[EGMClock], Optional[EGMTimeStamp]]:
         clock = None