├── KRL-Reference-Guide-v4_1.pdf ├── README.md ├── kukadriver ├── Kukapy-main.rar ├── mappdk_kuka_cmd.src ├── mappdk_kuka_comm.src ├── mappdk_kuka_logger.src ├── mappdk_kuka_server.src └── mappdk_kuka_utils.src └── kukapy ├── __init__.py ├── calibration.py ├── robot.py ├── robotapp.py └── transformations.py /KRL-Reference-Guide-v4_1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mf093087/KukaPy/0a0f32a4f55cd48fec869a86048c6690b206127e/KRL-Reference-Guide-v4_1.pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # kukapy 2 | 3 | A Python package for controlling KUKA robots by interfacing with the KUKA Controller. 4 | 5 | [![MIT License](https://img.shields.io/badge/License-MIT-yellow.svg)](LICENSE) 6 | [![Python Version](https://img.shields.io/badge/Python-3.x-blue.svg)](https://www.python.org/downloads/) 7 | 8 | ## Overview 9 | 10 | **kukapy** provides a robust Python API for controlling KUKA robots. It interfaces with the KUKA Controller via the `kukadriver` module and offers functionalities for calibration, motion control, and transformation handling. 11 | 12 | ## Table of Contents 13 | 14 | - [Project Structure](#project-structure) 15 | - [Installation](#installation) 16 | - [Deploying to KUKA Controller](#deploying-to-kuka-controller) 17 | - [Usage](#usage) 18 | - [Contributing](#contributing) 19 | - [License](#license) 20 | - [Contact](#contact) 21 | 22 | ## Project Structure 23 | 24 | ### kukadriver (KRL Scripts for the KUKA Robot Controller) 25 | - **`mappdk_kuka_cmd.src`** – Handles robot commands. 26 | - **`mappdk_kuka_comm.src`** – Manages communication between the robot and the external system. 27 | - **`mappdk_kuka_logger.src`** – Logs system and command activities. 28 | - **`mappdk_kuka_server.src`** – Main interface server for the robot. 29 | - **`mappdk_kuka_utils.src`** – Provides utility functions for robot operations. 30 | 31 | ### kukapy (Python API for KUKA Robot Interaction) 32 | - **`calibration.py`** – Handles robot calibration. 33 | - **`robot.py`** – Core module for robot control. 34 | - **`robotapp.py`** – Application-level control logic. 35 | - **`transformations.py`** – Utilities for handling transformations (e.g., quaternions, Euler angles). 36 | 37 | ## Installation 38 | 39 | 1. **Ensure Python 3.x is installed.** 40 | 2. **Clone the repository:** 41 | ```bash 42 | git clone https://github.com/your-username/kukapy.git 43 | cd kukapy 44 | Install dependencies: 45 | bash 46 | Copy 47 | pip install -r requirements.txt 48 | Deploying to KUKA Controller 49 | Transfer the .src files: 50 | Copy the files from the kukadriver directory onto the KUKA controller. 51 | Configure network communication: 52 | Set the appropriate IP addresses. 53 | Verify connectivity between the controller and your computer. 54 | Start the KUKA programs: 55 | Run the necessary scripts (e.g., mappdk_kuka_server.src) on the robot to enable command execution. 56 | Usage 57 | Below is a quick example to get started: 58 | 59 | python 60 | Copy 61 | from kukapy.robot import Robot 62 | 63 | # Initialize the robot connection 64 | robot = Robot(ip="192.168.1.100", port=12345) 65 | 66 | # Move the robot to a specified position (XYZ coordinates + Quaternion) 67 | robot.move_to([500, 0, 300], [0, 1, 0, 0]) 68 | 69 | # Retrieve the current position 70 | pos = robot.get_position() 71 | print("Current Position:", pos) 72 | 73 | # Disconnect from the robot 74 | robot.disconnect() 75 | Contributing 76 | Contributions are welcome! If you have suggestions, improvements, or bug fixes, please open an issue or submit a pull request. For larger changes, consider discussing your ideas first by opening an issue. 77 | 78 | License 79 | This project is licensed under the MIT License. 80 | 81 | Contact 82 | For any inquiries, please open an issue on this repository or contact the maintainers directly. 83 | -------------------------------------------------------------------------------- /kukadriver/Kukapy-main.rar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mf093087/KukaPy/0a0f32a4f55cd48fec869a86048c6690b206127e/kukadriver/Kukapy-main.rar -------------------------------------------------------------------------------- /kukadriver/mappdk_kuka_cmd.src: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /kukadriver/mappdk_kuka_comm.src: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /kukadriver/mappdk_kuka_logger.src: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /kukadriver/mappdk_kuka_server.src: -------------------------------------------------------------------------------- 1 | &ACCESS RVP 2 | &REL 1 3 | 4 | ; Author: Fan Mo 5 | ; Email: mf3696576@gmail.com 6 | ; Copyright: Fan Mo 7 | ; Description: Manufacturing Apps 8 | ; Development Kit (MAPPDK) 9 | ; Driver for KUKA robots. 10 | ; External function declarations assuming these are defined in other files 11 | EXT INT OPEN_COMM(VAR FILE, INT, INT) 12 | EXT INT CLOSE_COMM(VAR FILE, INT) 13 | EXT BOOL HANDLE_CMD(CHAR[254], CHAR[254]) 14 | EXT DEF MAPPDK_SERVER() 15 | 16 | DECL FILE comm_file 17 | DECL INT status 18 | DECL CHAR cmd[254] 19 | DECL CHAR resp[254] 20 | DECL BOOL keep_conn 21 | DECL INT SERVER_TAG_NUM = 8 ; Typically used for identifying server configurations if needed 22 | DECL INT PORT_NUMBER = 18735 23 | 24 | ; Start of main program 25 | BAS(#INITMOV,0) 26 | WRITE("MAPPDK SERVER started.\n") 27 | 28 | ; Main communication loop 29 | LOOP 30 | ; Attempt to open a communication channel on the specified port 31 | status = OPEN_COMM(comm_file, SERVER_TAG_NUM, PORT_NUMBER) 32 | IF status <> 0 THEN 33 | WRITE("Failed to open communication channel.\n") 34 | EXIT 35 | ENDIF 36 | 37 | ; Keep communication open until instructed to close 38 | keep_conn = TRUE 39 | WHILE keep_conn DO 40 | READ comm_file[] (cmd) 41 | IF cmd == "" THEN 42 | keep_conn = FALSE 43 | ELSE 44 | ; Process the command and determine if connection should remain open 45 | keep_conn = HANDLE_CMD(cmd, resp) 46 | ; Send a response back through the communication channel 47 | WRITE comm_file[] (resp) 48 | IF NOT keep_conn THEN 49 | ; Optional: Handle additional cleanup if needed 50 | WRITE comm_file[] ("Closing connection.\n") 51 | READ comm_file[] (cmd:0) ; Reading with timeout to ensure closure 52 | ENDIF 53 | ENDIF 54 | ENDWHILE 55 | 56 | ; Close the communication channel properly 57 | status = CLOSE_COMM(comm_file, SERVER_TAG_NUM) 58 | IF status <> 0 THEN 59 | WRITE("Failed to close communication channel.\n") 60 | ENDIF 61 | ENDLOOP 62 | 63 | WRITE("MAPPDK SERVER finished.\n") 64 | END MAPPDK_SERVER 65 | -------------------------------------------------------------------------------- /kukadriver/mappdk_kuka_utils.src: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /kukapy/__init__.py: -------------------------------------------------------------------------------- 1 | from fanucpy.robot import Robot 2 | from fanucpy.robotapp import RobotApp 3 | -------------------------------------------------------------------------------- /kukapy/calibration.py: -------------------------------------------------------------------------------- 1 | import os 2 | import pickle 3 | 4 | import numpy as np 5 | from cv2 import cv2 6 | 7 | 8 | def save_calib_data(calib_data: object, calib_data_path: str): 9 | """Saves calibration data.""" 10 | with open(calib_data_path, "wb") as f: 11 | pickle.dump(calib_data, f) 12 | 13 | 14 | def load_calib_data(calib_data_path: str): 15 | """Loads calibration data.""" 16 | with open(calib_data_path, "rb") as f: 17 | calib_data = pickle.load(f) 18 | return calib_data 19 | 20 | 21 | def draw_axis(img, corners, imgpts, thickness=5): 22 | """Draws xyz axis.""" 23 | # NOTE: opencv in BGR order, B-z, G-y, R-x 24 | corner = tuple(corners[0].astype(int).ravel()) 25 | img = cv2.line( 26 | img, 27 | corner, 28 | tuple(imgpts[0].astype(int).ravel()), 29 | (0, 0, 255), 30 | thickness=thickness, 31 | ) 32 | img = cv2.line( 33 | img, 34 | corner, 35 | tuple(imgpts[1].astype(int).ravel()), 36 | (0, 255, 0), 37 | thickness=thickness, 38 | ) 39 | img = cv2.line( 40 | img, 41 | corner, 42 | tuple(imgpts[2].astype(int).ravel()), 43 | (255, 0, 0), 44 | thickness=thickness, 45 | ) 46 | return img 47 | 48 | 49 | def collect_checker_board_images(camera, save_dir): 50 | """Collects checkerboard images.""" 51 | 52 | print("Type [c] to capture, [q] to exit.") 53 | count = 1 54 | while True: 55 | _, frame = camera.read() 56 | cv2.imshow("Frame", frame) 57 | 58 | key = cv2.waitKey(1) 59 | if key == ord("q"): 60 | break 61 | elif key == ord("c"): 62 | cv2.imwrite(os.path.join(save_dir, f"image_{count}.png"), frame) 63 | print(f"Saved data #{count}") 64 | count += 1 65 | 66 | camera.release() 67 | cv2.destroyAllWindows() 68 | 69 | 70 | def calibrate_camera_checkerboard(images, cols, rows, square_size, verbose=True): 71 | """Calibrates camera to get camera matrix and distortion coefficients.""" 72 | 73 | criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) 74 | 75 | # prepare object points 76 | objp = np.zeros((cols * rows, 3), np.float32) 77 | objp[:, :2] = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2) 78 | objp = objp * square_size 79 | 80 | # arrays to store object points and image points from all the images 81 | objpoints = [] 82 | imgpoints = [] 83 | 84 | for img in images: 85 | gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 86 | 87 | ret, corners = cv2.findChessboardCorners(gray, (cols, rows), None) 88 | 89 | if ret: 90 | objpoints.append(objp) 91 | 92 | corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) 93 | imgpoints.append(corners) 94 | 95 | out_img = img.copy() 96 | if verbose: 97 | cv2.drawChessboardCorners(out_img, (cols, rows), corners, ret) 98 | cv2.imshow("Calibration", out_img) 99 | cv2.waitKey(0) 100 | 101 | cv2.destroyAllWindows() 102 | 103 | rmse, camera_matrix, dist_coeffs, _, _ = cv2.calibrateCamera( 104 | objpoints, imgpoints, gray.shape[::-1], None, None 105 | ) 106 | 107 | return rmse, camera_matrix, dist_coeffs 108 | 109 | 110 | def find_aruco_pose(frame, camera_matrix, dist_coeffs, marker_length): 111 | """Finds aruco marker pose.""" 112 | gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) 113 | aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_7X7_50) 114 | parameters = cv2.aruco.DetectorParameters_create() 115 | corners, ids, _ = cv2.aruco.detectMarkers( 116 | gray, 117 | aruco_dict, 118 | parameters=parameters, 119 | cameraMatrix=camera_matrix, 120 | distCoeff=dist_coeffs, 121 | ) 122 | R_target2cam, t_target2cam = None, None 123 | if np.all(ids is not None): 124 | for i in range(len(ids)): 125 | rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers( 126 | corners=corners[i], 127 | markerLength=marker_length, 128 | cameraMatrix=camera_matrix, 129 | distCoeffs=dist_coeffs, 130 | ) 131 | cv2.aruco.drawDetectedMarkers(frame, corners) 132 | cv2.aruco.drawAxis( 133 | image=frame, 134 | cameraMatrix=camera_matrix, 135 | distCoeffs=dist_coeffs, 136 | rvec=rvec, 137 | tvec=tvec, 138 | length=50, 139 | ) 140 | R_target2cam, _ = cv2.Rodrigues(rvec) 141 | t_target2cam = tvec.squeeze() 142 | 143 | return R_target2cam, t_target2cam 144 | 145 | 146 | def find_checkerboard_pose(frame, camera_matrix, dist_coeffs, cols, rows, square_size): 147 | """Finds checkerboard pose.""" 148 | R_target2cam, t_target2cam = None, None 149 | 150 | criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) 151 | objp = np.zeros((cols * rows, 3), np.float32) 152 | objp[:, :2] = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2) 153 | objp = objp * square_size 154 | 155 | axis = np.float32( 156 | [[3 * square_size, 0, 0], [0, 3 * square_size, 0], [0, 0, -3 * square_size]] 157 | ).reshape(-1, 3) 158 | 159 | gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) 160 | 161 | ret, corners = cv2.findChessboardCorners(gray, (cols, rows), None) 162 | 163 | if ret: 164 | # refine corners 165 | corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) 166 | 167 | # find pose 168 | ret, rvec, tvec = cv2.solvePnP(objp, corners, camera_matrix, dist_coeffs) 169 | R_target2cam, _ = cv2.Rodrigues(rvec) 170 | t_target2cam = tvec.squeeze() 171 | 172 | imgpts, _ = cv2.projectPoints(axis, rvec, tvec, camera_matrix, dist_coeffs) 173 | frame = draw_axis(frame, corners, imgpts) 174 | 175 | return R_target2cam, t_target2cam 176 | 177 | 178 | def collect_eye_hand_data( 179 | camera, 180 | camera_matrix, 181 | dist_coeffs, 182 | marker_length, 183 | robot, 184 | aruco, 185 | cols, 186 | rows, 187 | square_size, 188 | ): 189 | """Collects eye hand calibration data.""" 190 | target_poses = [] 191 | robot_ee_poses = [] 192 | count = 0 193 | while True: 194 | _, frame = camera.read() 195 | if aruco: 196 | R_target2cam, t_target2cam = find_aruco_pose( 197 | frame=frame, 198 | camera_matrix=camera_matrix, 199 | dist_coeffs=dist_coeffs, 200 | marker_length=marker_length, 201 | ) 202 | else: 203 | R_target2cam, t_target2cam = find_checkerboard_pose( 204 | frame=frame, 205 | camera_matrix=camera_matrix, 206 | dist_coeffs=dist_coeffs, 207 | cols=cols, 208 | rows=rows, 209 | square_size=square_size, 210 | ) 211 | cv2.imshow("frame", frame) 212 | 213 | key = cv2.waitKey(1) 214 | if key == ord("q"): 215 | break 216 | if key == ord("c"): 217 | if R_target2cam is None: 218 | print("None pose detected, try again") 219 | else: 220 | target_poses.append((R_target2cam, t_target2cam)) 221 | 222 | robot.connect() 223 | xyzrpw = robot.get_curpos() 224 | robot_ee_poses.append(xyzrpw) 225 | count += 1 226 | print(f"Collected data: {count}") 227 | 228 | return target_poses, robot_ee_poses 229 | 230 | 231 | def calibrate_eye_hand( 232 | R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, eye_to_hand=True 233 | ): 234 | tts = [] 235 | if eye_to_hand: 236 | # change coordinates from gripper2base to base2gripper 237 | R_base2gripper, t_base2gripper = [], [] 238 | for R, t in zip(R_gripper2base, t_gripper2base): 239 | R_b2g = R.T 240 | t_b2g = -R_b2g @ t 241 | tts.append(t_b2g) 242 | R_base2gripper.append(R_b2g) 243 | t_base2gripper.append(t_b2g) 244 | 245 | # change parameters values 246 | R_gripper2base = R_base2gripper 247 | t_gripper2base = t_base2gripper 248 | 249 | # calibrate 250 | R, t = cv2.calibrateHandEye( 251 | R_gripper2base=R_gripper2base, 252 | t_gripper2base=t_gripper2base, 253 | R_target2cam=R_target2cam, 254 | t_target2cam=t_target2cam, 255 | ) 256 | 257 | return R, t 258 | -------------------------------------------------------------------------------- /kukapy/robot.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import socket 4 | from typing import Literal 5 | 6 | 7 | class FanucError(Exception): 8 | pass 9 | 10 | 11 | class Robot: 12 | def __init__( 13 | self, 14 | robot_model: str, 15 | host: str, 16 | port: int = 18375, 17 | ee_DO_type: str | None = None, 18 | ee_DO_num: int | None = None, 19 | socket_timeout: int = 60, 20 | ): 21 | """Class to connect to the robot, send commands, and receive 22 | responses. 23 | 24 | Args: 25 | robot_model (str): Robot model: Fanuc, Kuka, etc. 26 | host (str): IP address of host. 27 | port (int): Port number. Defaults to 18735. 28 | ee_DO_type (str, optional): End-effector digital output 29 | type. Fanuc used RDO type. Defaults to None. Others may 30 | use DO type. 31 | ee_DO_num (int, optional): End-effector digital output 32 | number. Defaults to None. 33 | socket_timeout(int): Socket timeout in seconds. Defaults to 34 | 5 seconds. 35 | """ 36 | self.robot_model = robot_model 37 | self.host = host 38 | self.port = port 39 | self.ee_DO_type = ee_DO_type 40 | self.ee_DO_num = ee_DO_num 41 | self.sock_buff_sz = 1024 42 | self.socket_timeout = socket_timeout 43 | self.comm_sock: socket.socket 44 | self.SUCCESS_CODE = 0 45 | self.ERROR_CODE = 1 46 | 47 | def handle_response( 48 | self, resp: str, continue_on_error: bool = False 49 | ) -> tuple[Literal[0, 1], str]: 50 | """Handles response from socket communication. 51 | 52 | Args: 53 | resp (str): Response string returned from socket. 54 | verbose (bool, optional): [description]. Defaults to False. 55 | 56 | Returns: 57 | tuple(int, str): Response code and response message. 58 | """ 59 | code_, msg = resp.split(":") 60 | code = int(code_) 61 | 62 | # Catch possible errors 63 | if code == self.ERROR_CODE and not continue_on_error: 64 | raise FanucError(msg) 65 | if code not in (self.SUCCESS_CODE, self.ERROR_CODE): 66 | raise FanucError(f"Unknown response code: {code} and message: {msg}") 67 | 68 | return code, msg # type: ignore[return-value] 69 | 70 | def connect(self) -> tuple[Literal[0, 1], str]: 71 | """Connects to the physical robot.""" 72 | self.comm_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 73 | self.comm_sock.settimeout(self.socket_timeout) 74 | self.comm_sock.connect((self.host, self.port)) 75 | resp = self.comm_sock.recv(self.sock_buff_sz).decode() 76 | return self.handle_response(resp) 77 | 78 | def disconnect(self) -> None: 79 | self.comm_sock.close() 80 | 81 | def send_cmd( 82 | self, cmd: str, continue_on_error: bool = False 83 | ) -> tuple[Literal[0, 1], str]: 84 | """Sends command to a physical robot. 85 | 86 | Args: 87 | cmd (str): Command string. 88 | 89 | Returns: 90 | tuple(int, str): Response code and response message. 91 | """ 92 | # end of command character 93 | cmd = cmd.strip() + "\n" 94 | 95 | # Send command 96 | self.comm_sock.sendall(cmd.encode()) 97 | 98 | # Wait for a result (blocking) 99 | resp = self.comm_sock.recv(self.sock_buff_sz).decode() 100 | return self.handle_response(resp=resp, continue_on_error=continue_on_error) 101 | 102 | def call_prog(self, prog_name: str) -> tuple[Literal[0, 1], str]: 103 | """Calls external program name in a physical robot. 104 | 105 | Args: 106 | prog_name ([str]): External program name. 107 | """ 108 | cmd = f"mappdkcall:{prog_name}" 109 | return self.send_cmd(cmd) 110 | 111 | def get_ins_power(self) -> float: 112 | """Gets instantaneous power consumption. 113 | 114 | Returns: 115 | float: Watts. 116 | """ 117 | 118 | cmd = "ins_pwr" 119 | _, msg = self.send_cmd(cmd) 120 | 121 | # Fanuc returns in kW. Should be adjusted to other robots. 122 | ins_pwr = float(msg) * 1000 123 | 124 | return ins_pwr 125 | 126 | def get_curpos(self) -> list[float]: 127 | """Gets current cartesian position of tool center point. 128 | 129 | Returns: 130 | list[float]: Current positions XYZWPR. 131 | """ 132 | 133 | cmd = "curpos" 134 | _, msg = self.send_cmd(cmd) 135 | vals = [float(val.split("=")[1]) for val in msg.split(",")] 136 | return vals 137 | 138 | def get_curjpos(self) -> list[float]: 139 | """Gets current joint values of tool center point. 140 | 141 | Returns: 142 | list[float]: Current joint values. 143 | """ 144 | cmd = "curjpos" 145 | _, msg = self.send_cmd(cmd) 146 | vals = [float(val.split("=")[1]) for val in msg.split(",") if val != "j=none"] 147 | return vals 148 | 149 | def move( 150 | self, 151 | move_type: Literal["joint"] | Literal["pose"], 152 | vals: list, 153 | velocity: int = 25, 154 | acceleration: int = 100, 155 | cnt_val: int = 0, 156 | linear: bool = False, 157 | continue_on_error: bool = False, 158 | ) -> tuple[Literal[0, 1], str]: 159 | """Moves robot. 160 | 161 | Args: 162 | move_type (str): Movement type (joint or pose). 163 | vals (list[real]): Position values. 164 | velocity (int, optional): Percentage or mm/s. Defaults to 165 | 25%. 166 | acceleration (int, optional): Percentage or mm/s^2. Defaults 167 | to 100%. 168 | cnt_val (int, optional): Continuous value for stopping. 169 | Defaults to 50. 170 | linear (bool, optioal): Linear movement. Defaults to False. 171 | 172 | Raises: 173 | ValueError: raised if movement type is not one of 174 | ("movej", "movep") 175 | """ 176 | 177 | # prepare velocity. percentage or mm/s 178 | # format: aaaa, e.g.: 0001%, 0020%, 3000 mm/s 179 | velocity = int(velocity) 180 | velocity_ = f"{velocity:04}" 181 | 182 | # prepare acceleration. percentage or mm/s^2 183 | # format: aaaa, e.g.: 0001%, 0020%, 0100 mm/s^2 184 | acceleration = int(acceleration) 185 | acceleration_ = f"{acceleration:04}" 186 | 187 | # prepare CNT value 188 | # format: aaa, e.g.: 001, 020, 100 189 | cnt_val = int(cnt_val) 190 | if not (0 <= cnt_val <= 100): 191 | raise ValueError("Incorrect CNT value.") 192 | cnt_val_ = f"{cnt_val:03}" 193 | 194 | if move_type == "joint" or move_type == "movej": 195 | cmd = "movej" 196 | elif move_type == "pose" or move_type == "movep": 197 | cmd = "movep" 198 | else: 199 | raise ValueError("Incorrect movement type!") 200 | 201 | motion_type = int(linear) 202 | 203 | cmd += f":{velocity_}:{acceleration_}:{cnt_val_}:{motion_type}:{len(vals)}" 204 | 205 | # prepare joint values 206 | for val in vals: 207 | vs = f"{abs(val):013.6f}" 208 | if val >= 0: 209 | vs = "+" + vs 210 | else: 211 | vs = "-" + vs 212 | cmd += f":{vs}" 213 | 214 | # call send_cmd 215 | return self.send_cmd(cmd, continue_on_error=continue_on_error) 216 | 217 | def gripper( 218 | self, 219 | value: bool, 220 | continue_on_error: bool = False, 221 | ) -> tuple[Literal[0, 1], str]: 222 | """Opens/closes robot gripper. 223 | 224 | Args: 225 | value (bool): True or False 226 | """ 227 | if (self.ee_DO_type is not None) and (self.ee_DO_num is not None): 228 | cmd = "" 229 | if self.ee_DO_type == "RDO": 230 | cmd = "setrdo" 231 | port = str(self.ee_DO_num) 232 | elif self.ee_DO_type == "DO": 233 | cmd = "setdout" 234 | port = str(self.ee_DO_num).zfill(5) 235 | else: 236 | raise ValueError("Wrong DO type!") 237 | 238 | cmd = cmd + f":{port}:{str(value).lower()}" 239 | return self.send_cmd(cmd, continue_on_error=continue_on_error) 240 | else: 241 | raise ValueError("DO type or number is None!") 242 | 243 | def get_rdo(self, rdo_num: int) -> int: 244 | """Get RDO value. 245 | 246 | Args: 247 | rdo_num (int): RDO number. 248 | 249 | Returns: 250 | rdo_value: RDO value. 251 | """ 252 | cmd = f"getrdo:{rdo_num}" 253 | _, rdo_value_ = self.send_cmd(cmd) 254 | rdo_value = int(rdo_value_) 255 | return rdo_value 256 | 257 | def set_rdo( 258 | self, 259 | rdo_num: int, 260 | val: bool, 261 | continue_on_error: bool = False, 262 | ) -> tuple[Literal[0, 1], str]: 263 | """Sets RDO value. 264 | 265 | Args: 266 | rdo_num (int): RDO number. 267 | val (bool): Value. 268 | """ 269 | cmd = f"setrdo:{rdo_num}:{str(val).lower()}" 270 | return self.send_cmd(cmd, continue_on_error=continue_on_error) 271 | 272 | def get_dout(self, dout_num: int) -> int: 273 | """Get DOUT value. 274 | 275 | Args: 276 | dout_num (int): DOUT number. 277 | 278 | Returns: 279 | dout_value: DOUT value. 280 | """ 281 | cmd = f"getdout:{str(dout_num).zfill(5)}" 282 | _, dout_value_ = self.send_cmd(cmd) 283 | dout_value = int(dout_value_) 284 | return dout_value 285 | 286 | def set_dout( 287 | self, 288 | dout_num: int, 289 | val: bool, 290 | continue_on_error: bool = False, 291 | ) -> tuple[Literal[0, 1], str]: 292 | """Sets DOUT value. 293 | 294 | Args: 295 | dout_num (int): DOUT number. 296 | val (bool): Value. 297 | """ 298 | cmd = f"setdout:{str(dout_num).zfill(5)}:{str(val).lower()}" 299 | return self.send_cmd(cmd, continue_on_error=continue_on_error) 300 | 301 | def set_sys_var( 302 | self, 303 | sys_var: str, 304 | val: bool, 305 | continue_on_error: bool = False, 306 | ) -> tuple[Literal[0, 1], str]: 307 | """Sets system variable to True or False. 308 | 309 | Args: 310 | sys_var (str): System variable name. 311 | val (bool): Value. 312 | """ 313 | val_ = "T" if val else "F" 314 | cmd = f"setsysvar:{sys_var}:{val_}" 315 | return self.send_cmd(cmd, continue_on_error=continue_on_error) 316 | 317 | 318 | if __name__ == "__main__": 319 | robot = Robot( 320 | robot_model="Fanuc", 321 | host="10.211.55.3", 322 | port=18735, 323 | ee_DO_type="RDO", 324 | ee_DO_num=7, 325 | ) 326 | 327 | robot.connect() 328 | 329 | # move in joint space 330 | robot.move( 331 | "joint", 332 | vals=[0, 0, 0, 0, 0, 0], 333 | velocity=100, 334 | acceleration=100, 335 | cnt_val=0, 336 | linear=not False, 337 | ) 338 | print(robot.get_curpos()) 339 | 340 | # move in cartesian space 341 | robot.move( 342 | "pose", 343 | vals=[350, 0, 280, -15, -90, -160], 344 | velocity=100, 345 | acceleration=100, 346 | cnt_val=0, 347 | linear=not False, 348 | ) 349 | -------------------------------------------------------------------------------- /kukapy/robotapp.py: -------------------------------------------------------------------------------- 1 | import traceback 2 | from abc import ABC, abstractmethod 3 | 4 | 5 | class RobotApp(ABC): 6 | """Generic Robot App Interface. 7 | Use this interface to develop robotic apps. 8 | """ 9 | 10 | def __init__(self) -> None: 11 | super().__init__() 12 | 13 | @abstractmethod 14 | def configure(self): 15 | raise NotImplementedError("Should implement configure method!") 16 | 17 | @abstractmethod 18 | def _main(self, **kwargs): 19 | """Main method""" 20 | raise NotImplementedError("Should implement run method!") 21 | 22 | def run(self, **kwargs): 23 | """Execution method.""" 24 | status = True 25 | message = "success" 26 | result = None 27 | try: 28 | result = self._main(**kwargs) 29 | except Exception as excp: 30 | status = False 31 | message = "".join( 32 | traceback.TracebackException.from_exception(excp).format() 33 | ) 34 | result = None 35 | finally: 36 | return status, message, result 37 | -------------------------------------------------------------------------------- /kukapy/transformations.py: -------------------------------------------------------------------------------- 1 | """Transformations related to the Fanuc robot. 2 | 3 | Some of the transformations are based on the following sources: 4 | - https://automaticaddison.com/how-to-convert-euler-angles-to-quaternions-using-python/ 5 | - https://automaticaddison.com/how-to-convert-a-quaternion-into-euler-angles-in-python/ 6 | """ 7 | import numpy as np 8 | from scipy.spatial.transform import Rotation 9 | 10 | import math 11 | from collections import namedtuple 12 | 13 | 14 | WPR = namedtuple("WPR", ["W", "P", "R"]) 15 | WrPrRr = namedtuple("WrPrRr", ["Wr", "Pr", "Rr"]) 16 | Quaternion = namedtuple("Quaternion", ["x", "y", "z", "w"]) 17 | 18 | 19 | def Rt_to_H(R, t): 20 | """Converts 3x3 rotation matrix 3x1 translation vector into 21 | 4x4 homogeneous transformation matrix.""" 22 | H = np.eye(4) 23 | H[0:3, 0:3] = R 24 | H[0:3, 3] = t.squeeze() 25 | return H 26 | 27 | 28 | def H_to_Rt(H): 29 | """Converts 4x4 homogeneous transformation matrix into 30 | 3x3 rotation matrix and 3x1 translation vector.""" 31 | return H[0:3, 0:3], H[0:3, 3] 32 | 33 | 34 | def xyzrpw_to_H(xyzrpw): 35 | """Converts xyzrpw to 4x4 homogeneous transformation matrix.""" 36 | R = Rotation.from_euler("xyz", xyzrpw[3:], degrees=True).as_matrix() 37 | H = Rt_to_H(R, xyzrpw[:3]) 38 | return H 39 | 40 | 41 | def H_to_xyzrpw(H): 42 | """Converts 4x4 homogeneous transformation matrix to xyzrpw.""" 43 | R, t = H_to_Rt(H) 44 | rpw = Rotation.from_matrix(R).as_euler("xyz", degrees=True) 45 | return list(t.flatten()) + list(rpw.flatten()) 46 | 47 | 48 | def WPR_to_WrPrRr(wpr: WPR) -> WrPrRr: 49 | """Converts WPR angles from degrees to radians. 50 | 51 | Args: 52 | wpr (WPR): WPR angles in degrees 53 | 54 | Returns: 55 | WrPrRr: WPR angles in radians 56 | """ 57 | return WrPrRr( 58 | Wr=math.radians(wpr.W), 59 | Pr=math.radians(wpr.P), 60 | Rr=math.radians(wpr.R), 61 | ) 62 | 63 | 64 | def WrPrRr_to_WPR(wpr: WrPrRr) -> WPR: 65 | """Converts WPR angles from radians to degrees. 66 | 67 | Args: 68 | wpr (WrPrRr): WPR angles in radians 69 | 70 | Returns: 71 | WPR: WPR angles in degrees 72 | """ 73 | return WPR( 74 | W=math.degrees(wpr.Wr), 75 | P=math.degrees(wpr.Pr), 76 | R=math.degrees(wpr.Rr), 77 | ) 78 | 79 | 80 | def WrPrRr_to_Quaternion(wpr: WrPrRr) -> Quaternion: 81 | """Converts WrPrRr angles to a quaternion. 82 | 83 | https://automaticaddison.com/how-to-convert-euler-angles-to-quaternions-using-python/ 84 | 85 | Args: 86 | wpr (WPR): WPR angles in radians 87 | 88 | Returns: 89 | Quaternion: WPR angles as a quaternion 90 | """ 91 | return Quaternion( 92 | x=math.sin(wpr.Rr / 2) * math.cos(wpr.Pr / 2) * math.cos(wpr.Wr / 2) 93 | - math.cos(wpr.Rr / 2) * math.sin(wpr.Pr / 2) * math.sin(wpr.Wr / 2), 94 | y=math.cos(wpr.Rr / 2) * math.sin(wpr.Pr / 2) * math.cos(wpr.Wr / 2) 95 | + math.sin(wpr.Rr / 2) * math.cos(wpr.Pr / 2) * math.sin(wpr.Wr / 2), 96 | z=math.cos(wpr.Rr / 2) * math.cos(wpr.Pr / 2) * math.sin(wpr.Wr / 2) 97 | - math.sin(wpr.Rr / 2) * math.sin(wpr.Pr / 2) * math.cos(wpr.Wr / 2), 98 | w=math.cos(wpr.Rr / 2) * math.cos(wpr.Pr / 2) * math.cos(wpr.Wr / 2) 99 | + math.sin(wpr.Rr / 2) * math.sin(wpr.Pr / 2) * math.sin(wpr.Wr / 2), 100 | ) 101 | 102 | 103 | def Quaternion_to_WrPrRr(quat: Quaternion) -> WrPrRr: 104 | """Converts a quaternion to WrPrRr angles. 105 | 106 | https://automaticaddison.com/how-to-convert-a-quaternion-into-euler-angles-in-python/ 107 | 108 | Args: 109 | quat (Quaternion): WPR angles as a quaternion 110 | 111 | Returns: 112 | WrPrRr: WPR angles in radians 113 | """ 114 | return WrPrRr( 115 | Wr=math.atan2( 116 | 2 * (quat.w * quat.z + quat.x * quat.y), 1 - 2 * (quat.y**2 + quat.z**2) 117 | ), 118 | Pr=math.asin(max(min(2 * (quat.w * quat.y - quat.z * quat.x), 1.0), -1.0)), 119 | Rr=math.atan2( 120 | 2 * (quat.w * quat.x + quat.y * quat.z), 1 - 2 * (quat.x**2 + quat.y**2) 121 | ), 122 | ) 123 | 124 | 125 | def WPR_to_Quaternion(wpr: WPR) -> Quaternion: 126 | """Converts WPR angles to a quaternion. 127 | 128 | https://automaticaddison.com/how-to-convert-euler-angles-to-quaternions-using-python/ 129 | 130 | Args: 131 | wpr (WPR): WPR angles in degrees 132 | 133 | Returns: 134 | Quaternion: WPR angles as a quaternion 135 | """ 136 | return WrPrRr_to_Quaternion(WPR_to_WrPrRr(wpr)) 137 | 138 | 139 | def Quaternion_to_WPR(quat: Quaternion) -> WPR: 140 | """Converts a quaternion to WPR angles. 141 | 142 | https://automaticaddison.com/how-to-convert-a-quaternion-into-euler-angles-in-python/ 143 | 144 | Args: 145 | quat (Quaternion): WPR angles as a quaternion 146 | 147 | Returns: 148 | WPR: WPR angles in degrees 149 | """ 150 | return WrPrRr_to_WPR(Quaternion_to_WrPrRr(quat)) 151 | --------------------------------------------------------------------------------