#! /usr/bin/env python import socket import json import time DEBUG: bool = True RESET = "\033[0m" CYAN = "\033[36m" YELLOW = "\033[33m" RED = "\033[31m" class RobotStatus: Stopped = 0 Running = 1 Paused = 2 Resumed = 3 class RobotJasonServerError: RESUTL_SUCC = 0 RESUTL_BASE = 3000 RESUTL_ERROR_BAD_COMMAND_STRING = RESUTL_BASE + 1 RESUTL_ERROR_UNKNOWN_COMMAND = RESUTL_BASE + 2 RESUTL_ERROR_ARGS = RESUTL_BASE + 2 RESUTL_ERROR_MOVE = RESUTL_BASE + 3 RESUTL_CALL_BACK = RESUTL_BASE + 4 RESUTL_ERROR_CLIENTS_NUMBER = RESUTL_BASE + 5 RESUTL_ERROR_DISCONNECT_TO_MODBUS_SERVER = RESUTL_BASE + 6 RESUTL_ERROR_CONNECT_TO_MODBUS_SERVER = RESUTL_BASE + 7 class RobotClient: def __init__(self, host, port): self.host = host self.port = port self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.sock.connect((self.host, self.port)) def _send(self, cmd) -> dict: if DEBUG: print(f"{CYAN}DEBUG: sended:{RESET} {cmd}") self.sock.sendall(json.dumps(cmd).encode("utf-8")) while True: data = self.sock.recv(4096).decode("utf-8") ret = json.loads(data) if DEBUG: print(f"{YELLOW}DEBUG: received:{RESET} {ret}") if ret["ret"] == RobotJasonServerError.RESUTL_CALL_BACK: if DEBUG: print(f"{YELLOW}DEBUG: received callback event (ignored as response): {ret['msg']}") continue else: return ret def rpy_to_quat(self, arr): cmd = {"command": "rpy_to_quaternion", "rpy": arr} ret = self._send(cmd) return ret["ret"] def get_current_waypoint(self): cmd = {"command": "get_current_waypoint"} ret = self._send(cmd) return ret["ret"]["joint"], ret["ret"]["pos"], ret["ret"]["ori"] def inverse_kin(self, joint_radian: list, pos: list, ori: list) -> list: """ Выполняет обратную кинематику, возвращает углы суставов (в радианах). :param joint_radian: текущая поза (6 углов в рад) :param pos: целевая позиция [x, y, z] в метрах :param ori: ориентация в виде кватерниона [w, x, y, z] :return: список из 6 углов суставов (в радианах) """ cmd = { "command": "inverse_kin", "joint_radian": joint_radian, "pos": pos, "ori": ori, } ret = self._send(cmd) if ret["msg"] != "succ": raise RuntimeError(f"Inverse kinematics failed: {ret}. \n Was sended {cmd}") return ret["ret"][ "joint" ] # возвращается как {"joint": [...], "pos": [...], "ori": [...]} def move_to_pose(self, pose: dict): """ Перемещает робота в указанную позицию и ориентацию. :param command: словарь с ключами "pos" и "ori" (ори — в радианах, RPY) """ # quat = self.rpy_to_quat(command["ori"]) # 2. Получаем текущее состояние робота joint_start, _, _ = self.get_current_waypoint() # 3. Вычисляем инверсную кинематику joint_target = self.inverse_kin(joint_start, pose["pos"], pose["ori"]) # 4. Выполняем линейное перемещение self.move_line(joint_target) def add_waypoint(self, joint_radian: list): cmd = {"command": "add_waypoint", "joint_radian": joint_radian} ret = self._send(cmd) if (ret.get("msg") != "succ"): raise RuntimeError(f"add_waypoint failed: {ret}") def move_track(self, track = "CARTESIAN_MOVEP"): cmd = {"command": "move_track", "track": track} ret = self._send(cmd) if (ret.get("msg") != "succ"): raise RuntimeError(f"move_track failed: {ret}") def move_linear_cartesian(self, pose: dict): """ Выполняет линейное движение в декартовом пространстве между текущей позицией и заданной позой. :param pose: словарь с ключами "pos" (XYZ в метрах) и "ori" (кватернион [w, x, y, z]) """ # 1. Получаем текущее состояние joint_start, _, _ = self.get_current_waypoint() # 2. Добавляем текущую позицию как waypoint self.add_waypoint(joint_start) # 3. Вычисляем IK joint_target = self.inverse_kin(joint_start, pose["pos"], pose["ori"]) # 4. Добавляем целевую позицию как waypoint self.add_waypoint(joint_target) # 5. Выполняем траекторию self.move_track() def get_robot_state(self) -> int: """ Получает текущее состояние робота. :return: Одно из значений: 0 — Stopped 1 — Running 2 — Paused 3 — Resumed :raises RuntimeError: если не удалось получить состояние """ cmd = {"command": "get_robot_state"} ret = self._send(cmd) if ret.get("msg") != "succ": raise RuntimeError(f"get_robot_state failed: {ret}") return ret["ret"] def move_line(self, joint_radian: list) -> None: """ Выполняет линейное перемещение в заданную позу, сохраняя текущую ориентацию. :param joint_radian: целевая поза (список из 6 значений в радианах) :raises RuntimeError: если команда не была выполнена успешно """ cmd = {"command": "move_line", "joint_radian": joint_radian} ret = self._send(cmd) if ret.get("ret") == 3004: return if ret.get("msg") != "succ": raise RuntimeError(f"move_line failed: {ret}") def set_joint_maxacc(self, acc: float): """ Устанавливает одинаковое максимальное ускорение для всех суставов (в rad/s²). :param acc: ускорение для всех 6 суставов :raises RuntimeError: если команда завершилась с ошибкой """ cmd = {"command": "set_joint_maxacc", "joint_maxacc": [acc] * 6} ret = self._send(cmd) if ret.get("msg") != "succ": raise RuntimeError(f"set_joint_maxacc failed: {ret}") def set_joint_maxvelc(self, vel: float): """ Устанавливает одинаковую максимальную скорость для всех суставов (в rad/s). :param vel: скорость для всех 6 суставов :raises RuntimeError: если команда завершилась с ошибкой """ cmd = {"command": "set_joint_maxvelc", "joint_maxvelc": [vel] * 6} ret = self._send(cmd) if ret.get("msg") != "succ": raise RuntimeError(f"set_joint_maxvelc failed: {ret}") def close(self): self.sock.close()