From 98110dd4547746136ba936b1bd252da41bfa62ce Mon Sep 17 00:00:00 2001 From: MrCybernetic <121680049+FLo-ABB@users.noreply.github.com> Date: Thu, 17 Apr 2025 19:33:26 +0200 Subject: [PATCH 1/2] init path correction exemple file --- examples/simple/example_path_correction.py | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 examples/simple/example_path_correction.py diff --git a/examples/simple/example_path_correction.py b/examples/simple/example_path_correction.py new file mode 100644 index 0000000..dfce4b2 --- /dev/null +++ b/examples/simple/example_path_correction.py @@ -0,0 +1,17 @@ +from ABBRobotEGM import EGM + +# Robot Module # +# MODULE MainModule +# VAR egmident egmID1; + +# PROC main() +# !TODO +# ENDPROC +# ENDMODULE + + +def main() -> None: + """ + TODO + """ + pass From 1dc8d654533b758ae7c5afe99ecb836ff9e10d77 Mon Sep 17 00:00:00 2001 From: MrCybernetic <121680049+FLo-ABB@users.noreply.github.com> Date: Sun, 20 Apr 2025 14:32:19 +0200 Subject: [PATCH 2/2] Enhance example_path_correction.py with sinusoidal path correction logic and improve EGM class method for path correction validation --- examples/simple/example_path_correction.py | 69 +++++++++++++++++++--- src/ABBRobotEGM/egm.py | 45 +++++++++----- 2 files changed, 92 insertions(+), 22 deletions(-) diff --git a/examples/simple/example_path_correction.py b/examples/simple/example_path_correction.py index dfce4b2..fbece24 100644 --- a/examples/simple/example_path_correction.py +++ b/examples/simple/example_path_correction.py @@ -1,17 +1,72 @@ from ABBRobotEGM import EGM +import numpy as np +import time -# Robot Module # +# 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; +# 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() -# !TODO -# ENDPROC +# 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: """ - TODO + 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. """ - pass + 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() diff --git a/src/ABBRobotEGM/egm.py b/src/ABBRobotEGM/egm.py index 837d684..6fac331 100644 --- a/src/ABBRobotEGM/egm.py +++ b/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