diff --git a/src/iiwa_sunrise/src/ServerFriRos2.java b/src/iiwa_sunrise/src/ServerFriRos2.java new file mode 100644 index 0000000..fd2b3df --- /dev/null +++ b/src/iiwa_sunrise/src/ServerFriRos2.java @@ -0,0 +1,529 @@ +package ros; + +// ════════════════════════════════════════════════════════════════════════════ +// +// ServerFriRos2.java +// Sunrise OS 1.16 | FRI 1.16 | KUKA iiwa 7 +// +// Режимы управления: +// Position — FRI задаёт целевые углы суставов (PositionControlMode) +// Сглаживание команд выполняется на стороне ROS2 (FRIClient EMA) +// Torque — FRI задаёт добавочные моменты (JointImpedanceControlMode) +// Monitor — FRI только читает состояние (NO_COMMAND_MODE) +// Масса инструмента берётся из Sunrise WB (Load Data) +// и валидируется через SmartServo.validateForImpedanceMode() +// +// Сетевые интерфейсы: +// KONI — 192.170.10.10 (рекомендуется) +// KLI — 192.168.21.31 +// +// ════════════════════════════════════════════════════════════════════════════ + +import java.util.concurrent.TimeUnit; +import java.util.concurrent.TimeoutException; + +import javax.inject.Inject; +import javax.inject.Named; + +import com.kuka.connectivity.fastRobotInterface.ClientCommandMode; +import com.kuka.connectivity.fastRobotInterface.FRIChannelInformation; +import com.kuka.connectivity.fastRobotInterface.FRIConfiguration; +import com.kuka.connectivity.fastRobotInterface.FRIJointOverlay; +import com.kuka.connectivity.fastRobotInterface.FRISession; +import com.kuka.connectivity.fastRobotInterface.IFRISessionListener; +import com.kuka.connectivity.motionModel.smartServo.SmartServo; +import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; +import com.kuka.roboticsAPI.controllerModel.Controller; +import com.kuka.roboticsAPI.deviceModel.LBR; +import com.kuka.roboticsAPI.geometricModel.Tool; +import com.kuka.roboticsAPI.motionModel.BasicMotions; +import com.kuka.roboticsAPI.motionModel.PositionHold; +import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; +import com.kuka.roboticsAPI.uiModel.ApplicationDialogType; + + +public class ServerFriRos2 extends RoboticsAPIApplication { + + + // ── КОНСТАНТЫ ──────────────────────────────────────────────────────────── + + private static final double[] ZERO_POSITION = {0, 0, 0, 0, 0, 0, 0}; + private static final double[] MONITOR_WORKING_POSITION = {0, 0, 0, -1.57, 0, 1.57, 0}; + + private static final String KONI_IP = "192.170.10.10"; + private static final String KLI_IP = "192.168.21.31"; + + private static final int FRI_CONNECT_TIMEOUT_SEC = 30; + private static final double APPROACH_VEL = 0.30; + + // Monitor режим: нулевая жёсткость = свободное ведение рукой + private static final double MONITOR_JOINT_STIFFNESS = 0.0; + private static final double MONITOR_JOINT_DAMPING = 0.7; + + + // ── ПЕРЕЧИСЛЕНИЯ ───────────────────────────────────────────────────────── + + private enum NetworkInterface { + KONI("KONI (192.170.10.10) - выделенная FRI-сеть", KONI_IP), + KLI ("KLI (192.168.21.31) - основная сеть KRC", KLI_IP); + + final String label; + final String ip; + + NetworkInterface(String label, String ip) { + this.label = label; + this.ip = ip; + } + } + + private enum ControlMode { + POSITION, + TORQUE, + MONITOR + } + + + // ── ПОЛЯ ───────────────────────────────────────────────────────────────── + + private LBR _lbr; + private Controller _lbrController; + + /** + * Инструмент, настроенный в Sunrise WB → Object Templates. + * Имя должно совпадать с именем в SWB (здесь: "patron"). + * Масса и CoM берутся из Load Data этого объекта. + */ + @Inject + @Named("patron") + private Tool _tool; + + private NetworkInterface _selectedNetwork; + private ControlMode _selectedMode; + private int _sendPeriodMs; + private double _jointStiffness; + + private FRIConfiguration _friConfig; + private FRISession _friSession; + private FRIJointOverlay _friOverlay; + private IFRISessionListener _friListener; + + + // ── LIFECYCLE ──────────────────────────────────────────────────────────── + + @Override + public void initialize() { + + _lbrController = (Controller) getContext().getControllers().toArray()[0]; + _lbr = (LBR) _lbrController.getDevices().toArray()[0]; + + // Прикрепляем инструмент к фланцу — обязательно для корректной + // гравкомпенсации и validateForImpedanceMode() + _tool.attachTo(_lbr.getFlange()); + + getLogger().info("════════════════════════════════════════════"); + getLogger().info(" ServerFriRos2 | KUKA iiwa 7 + ROS2 FRI"); + getLogger().info(" Sunrise OS 1.16 | FRI 1.16"); + getLogger().info(" Робот : " + _lbr.getName()); + getLogger().info(" Инструмент : " + _tool.getName()); + getLogger().info("════════════════════════════════════════════"); + + initFriListener(); + requestUserConfig(); + } + + @Override + public void run() throws Exception { + + moveToInitialPosition(); + + switch (_selectedMode) { + case POSITION: runPositionMode(); break; + case TORQUE: runTorqueMode(); break; + case MONITOR: runMonitorMode(); break; + } + + getLogger().info("Программа завершена."); + } + + @Override + public void dispose() { + + if (_friSession != null) { + getLogger().info("dispose(): Закрытие FRI-сессии..."); + try { + _friSession.close(); + } catch (Exception e) { + getLogger().warn("Ошибка при закрытии FRI: " + e.getMessage()); + } + _friSession = null; + } + + super.dispose(); + } + + + // ── UI — ЗАПРОС КОНФИГУРАЦИИ ───────────────────────────────────────────── + + private void requestUserConfig() { + + // Шаг 1: Сетевой интерфейс + int netChoice = getApplicationUI().displayModalDialog( + ApplicationDialogType.QUESTION, + "Шаг 1 / 3 — Сетевой интерфейс FRI\n\n" + + "KONI: выделенная высокоскоростная сеть (рекомендуется)\n" + + "KLI : основная сеть KRC", + NetworkInterface.KONI.label, + NetworkInterface.KLI.label + ); + _selectedNetwork = (netChoice == 0) ? NetworkInterface.KONI : NetworkInterface.KLI; + getLogger().info("[Шаг 1] Сеть: " + _selectedNetwork.label); + + // Шаг 2: Режим управления + int modeChoice = getApplicationUI().displayModalDialog( + ApplicationDialogType.QUESTION, + "Шаг 2 / 3 — Режим управления FRI\n\n" + + "Position : ROS2 задаёт угловые позиции суставов\n" + + "Torque : ROS2 задаёт добавочные моменты\n" + + "Monitor : только данные; ведение рукой", + "Position", + "Torque", + "Monitor" + ); + + if (modeChoice == 0) { + _selectedMode = ControlMode.POSITION; + _jointStiffness = 0; + + int pChoice = getApplicationUI().displayModalDialog( + ApplicationDialogType.QUESTION, + "Шаг 3 / 3 — Период отправки [мс] (Position)\n\n" + + "Режим: PositionControlMode (жёсткое позиционирование)\n" + + "Сглаживание команд выполняется на стороне ROS2.\n\n" + + "10 мс — стабильно\n" + + " 5 мс — стандарт для 200 Гц ros2_control\n" + + " 2 мс — быстро, требует низкого джиттера", + "10 мс", + " 5 мс", + " 2 мс" + ); + _sendPeriodMs = new int[]{10, 5, 2}[pChoice]; + + } else if (modeChoice == 1) { + _selectedMode = ControlMode.TORQUE; + + int pChoice = getApplicationUI().displayModalDialog( + ApplicationDialogType.QUESTION, + "Шаг 3а / 4 — Период отправки [мс] (Torque)\n\n" + + "При пропуске пакета Sunrise переходит в PositionHold.\n" + + "Рекомендуется 1–2 мс при стабильной KONI-сети.", + " 5 мс", + " 2 мс (рекомендуется)", + " 1 мс (максимальная частота)" + ); + _sendPeriodMs = new int[]{5, 2, 1}[pChoice]; + + int sChoice = getApplicationUI().displayModalDialog( + ApplicationDialogType.QUESTION, + "Шаг 3б / 4 — Жёсткость суставов [Нм/рад] (Torque)\n\n" + + "Высокая: точное следование, меньше отклонение\n" + + "Низкая : мягкое взаимодействие со средой\n\n" + + "⚠ 1500 Нм/рад — только без людей в рабочей зоне!", + "1500 (жёсткий / производственный)", + "1000 (стандарт)", + " 800 (средний)", + " 500 (мягкий / взаимодействие)", + " 300 (очень мягкий)" + ); + _jointStiffness = new double[]{1500, 1000, 800, 500, 300}[sChoice]; + + } else { + _selectedMode = ControlMode.MONITOR; + _sendPeriodMs = 2; // 2 мс — запас для non-RT систем без FIFO-планировщика + _jointStiffness = 0; + getLogger().info("[Шаг 3] Monitor: период = " + _sendPeriodMs + " мс"); + } + + getLogger().info("════════════════════════════════════════════"); + getLogger().info(" КОНФИГУРАЦИЯ ЗАПУСКА"); + getLogger().info(" Сеть : " + _selectedNetwork.label); + getLogger().info(" IP хоста : " + _selectedNetwork.ip); + getLogger().info(" Режим : " + _selectedMode.name()); + getLogger().info(" Период : " + _sendPeriodMs + " мс"); + getLogger().info(" Инструмент : " + _tool.getName()); + if (_selectedMode == ControlMode.TORQUE) { + getLogger().info(" Жёсткость : " + _jointStiffness + " Нм/рад"); + } + getLogger().info("════════════════════════════════════════════"); + } + + + // ── ДВИЖЕНИЕ В СТАРТОВУЮ ПОЗИЦИЮ ───────────────────────────────────────── + + private void moveToInitialPosition() { + + if (_selectedMode == ControlMode.MONITOR) { + + getLogger().info("Движение в рабочую позицию Monitor (через нулевую)..."); + _lbr.move( + BasicMotions.batch( + BasicMotions.ptp(ZERO_POSITION).setBlendingRel(0.5), + BasicMotions.ptp(MONITOR_WORKING_POSITION) + ).setJointVelocityRel(APPROACH_VEL) + ); + getLogger().info("Рабочая позиция Monitor достигнута."); + + } else { + + getLogger().info("Движение в нулевую позицию..."); + _lbr.move( + BasicMotions.ptp(ZERO_POSITION).setJointVelocityRel(APPROACH_VEL) + ); + getLogger().info("Нулевая позиция достигнута."); + } + } + + + // ── РЕЖИМ: POSITION ────────────────────────────────────────────────────── + + private void runPositionMode() { + getLogger().info("═══ Position режим (PositionControlMode) ═══"); + + if (!setupFriSession(ClientCommandMode.POSITION)) { + return; + } + + PositionControlMode ctrlMode = new PositionControlMode(); + PositionHold posHold = new PositionHold(ctrlMode, -1, TimeUnit.SECONDS); + + getLogger().info("Position режим активен. Ожидаю команды от ROS2..."); + + _lbr.move(posHold.addMotionOverlay(_friOverlay)); + + _friSession.close(); + _friSession = null; + getLogger().info("Position режим завершён. FRI закрыт."); + } + + + // ── РЕЖИМ: TORQUE ──────────────────────────────────────────────────────── + + private void runTorqueMode() { + getLogger().info("═══ Torque режим ═══"); + getLogger().info("Жёсткость: " + _jointStiffness + " Нм/рад"); + + if (!setupFriSession(ClientCommandMode.TORQUE)) { + return; + } + + JointImpedanceControlMode ctrlMode = new JointImpedanceControlMode( + _jointStiffness, _jointStiffness, _jointStiffness, + _jointStiffness, _jointStiffness, _jointStiffness, + _jointStiffness + ); + ctrlMode.setDampingForAllJoints(0.7); + + PositionHold posHold = new PositionHold(ctrlMode, -1, TimeUnit.SECONDS); + + getLogger().info("Torque режим активен. Ожидаю команды от ROS2..."); + + _lbr.move(posHold.addMotionOverlay(_friOverlay)); + + _friSession.close(); + _friSession = null; + getLogger().info("Torque режим завершён. FRI закрыт."); + } + + + // ── РЕЖИМ: MONITOR ─────────────────────────────────────────────────────── + + /** + * Monitor режим. + * + * Фаза A: Валидация Load Data инструмента из Sunrise WB. + * Масса берётся из Object Templates → patron → Load Data, + * как в TeachKuka.java (SmartServo.validateForImpedanceMode). + * + * Фаза B: Диалог подтверждения — оператор запускает ROS2 FRI узел. + * + * Фаза C: FRI в NO_COMMAND_MODE — данные суставов идут в ROS2. + * + * Фаза D: PositionHold с нулевой жёсткостью — свободное ведение рукой. + */ + private void runMonitorMode() { + getLogger().info("═══ Monitor режим ═══"); + + // Фаза A: валидация Load Data инструмента из SWB + validateLoadModel(); + + // Фаза B: ждём подтверждения оператора что ROS2 FRI готов + getApplicationUI().displayModalDialog( + ApplicationDialogType.INFORMATION, + "Запустите ROS2 FRI узел на ПК (" + _selectedNetwork.ip + ").\n\n" + + "Нажмите OK когда ros2_control_node активен.", + "OK — ROS2 готов" + ); + + // Фаза C: FRI в режиме только чтения + if (!setupFriSession(ClientCommandMode.NO_COMMAND_MODE)) { + return; + } + + // Фаза D: PositionHold с нулевой жёсткостью — свободное ведение рукой + JointImpedanceControlMode guidingMode = new JointImpedanceControlMode( + MONITOR_JOINT_STIFFNESS, MONITOR_JOINT_STIFFNESS, MONITOR_JOINT_STIFFNESS, + MONITOR_JOINT_STIFFNESS, MONITOR_JOINT_STIFFNESS, MONITOR_JOINT_STIFFNESS, + MONITOR_JOINT_STIFFNESS + ); + guidingMode.setDampingForAllJoints(MONITOR_JOINT_DAMPING); + + PositionHold posHold = new PositionHold(guidingMode, -1, TimeUnit.SECONDS); + + getLogger().info("Monitor активен:"); + getLogger().info(" • Ведите робота рукой — он не сопротивляется."); + getLogger().info(" • Данные суставов транслируются в ROS2 каждые " + + _sendPeriodMs + " мс."); + getLogger().info(" • Остановите FRI-клиент для завершения."); + + _lbr.move(posHold); + + _friSession.close(); + _friSession = null; + getLogger().info("Monitor режим завершён. FRI закрыт."); + } + + + // ── ВАЛИДАЦИЯ НАГРУЗКИ ИНСТРУМЕНТА ────────────────────────────────────── + + /** + * Проверяет Load Data (масса / CoM / инерция) инструмента из Sunrise WB. + * + * Аналог validateLoadModel() из TeachKuka.java: + * SmartServo.validateForImpedanceMode(_tool) проверяет, что данные + * нагрузки заданы корректно для работы с JointImpedanceControlMode. + * + * Если валидация не прошла — нужно задать Load Data в: + * Sunrise WB → Object Templates → patron → Load Data + * (Mass, Centre of Mass, Moment of Inertia) + */ + private void validateLoadModel() { + getLogger().info("════════════════════════════════════════════"); + getLogger().info(" ВАЛИДАЦИЯ НАГРУЗКИ ИНСТРУМЕНТА"); + getLogger().info(" Инструмент: " + _tool.getName()); + getLogger().info("════════════════════════════════════════════"); + + boolean valid = SmartServo.validateForImpedanceMode(_tool); + + if (valid) { + getLogger().info(" ✓ Load Data валидны."); + getLogger().info(" Масса и CoM заданы корректно в Sunrise WB."); + getLogger().info(" Гравкомпенсация будет работать точно."); + } else { + getLogger().warn(" ⚠ Валидация Load Data НЕ прошла!"); + getLogger().warn(" Задайте данные нагрузки в:"); + getLogger().warn(" Sunrise WB → Object Templates → " + + _tool.getName() + " → Load Data"); + getLogger().warn(" (Mass [кг], Centre of Mass [мм], Inertia [кг·м²])"); + getLogger().warn(" Гравкомпенсация в Monitor режиме может работать некорректно."); + + // Предупреждаем оператора — он решает продолжить или нет + int choice = getApplicationUI().displayModalDialog( + ApplicationDialogType.QUESTION, + "Load Data инструмента '" + _tool.getName() + "' не заданы.\n\n" + + "Без корректных данных нагрузки гравкомпенсация\n" + + "будет работать с ошибкой.\n\n" + + "Задайте данные в Sunrise WB → Object Templates → " + + _tool.getName() + " → Load Data,\nзатем перезапустите программу.\n\n" + + "Или продолжите без корректной нагрузки (на свой риск).", + "Продолжить", + "Остановить" + ); + + if (choice == 1) { + throw new RuntimeException( + "Остановлено оператором: Load Data не заданы для " + + _tool.getName()); + } + } + + getLogger().info("════════════════════════════════════════════"); + } + + + // ── FRI — НАСТРОЙКА СЕССИИ ─────────────────────────────────────────────── + + private boolean setupFriSession(ClientCommandMode commandMode) { + + _friConfig = FRIConfiguration.createRemoteConfiguration(_lbr, _selectedNetwork.ip); + _friConfig.setSendPeriodMilliSec(_sendPeriodMs); + _friConfig.setReceiveMultiplier(1); + + getLogger().info("Создание FRI-сессии..."); + getLogger().info(" Хост : " + _friConfig.getHostName() + + " порт: " + _friConfig.getPortOnRemote()); + getLogger().info(" Режим : " + commandMode.name()); + getLogger().info(" Период : " + _friConfig.getSendPeriodMilliSec() + " мс"); + + _friSession = new FRISession(_friConfig); + _friSession.addFRISessionListener(_friListener); + + try { + getLogger().info("Ожидание FRI-клиента на " + _selectedNetwork.ip + + " (таймаут " + FRI_CONNECT_TIMEOUT_SEC + " с)..."); + _friSession.await(FRI_CONNECT_TIMEOUT_SEC, TimeUnit.SECONDS); + + } catch (TimeoutException e) { + getLogger().error("Таймаут FRI! Клиент не ответил за " + + FRI_CONNECT_TIMEOUT_SEC + " с."); + getLogger().error("Убедитесь, что ROS2 FRI-узел запущен на " + + _selectedNetwork.ip); + _friSession.close(); + _friSession = null; + return false; + } + + getLogger().info("FRI-соединение установлено!"); + logFriChannelInfo(); + + if (commandMode != ClientCommandMode.NO_COMMAND_MODE) { + _friOverlay = new FRIJointOverlay(_friSession, commandMode); + getLogger().info("FRIJointOverlay создан: " + commandMode.name()); + } else { + _friOverlay = null; + getLogger().info("Monitor: FRIJointOverlay не создаётся (только чтение)."); + } + + return true; + } + + + // ── FRI — LISTENER ─────────────────────────────────────────────────────── + + private void initFriListener() { + _friListener = new IFRISessionListener() { + + @Override + public void onFRIConnectionQualityChanged(FRIChannelInformation info) { + getLogger().info("[FRI] Качество: " + info.getQuality() + + " jitter=" + info.getJitter() + " мс" + + " latency=" + info.getLatency() + " мс"); + } + + @Override + public void onFRISessionStateChanged(FRIChannelInformation info) { + getLogger().info("[FRI] Состояние: " + info.getFRISessionState() + + " jitter=" + info.getJitter() + " мс" + + " latency=" + info.getLatency() + " мс"); + } + }; + } + + private void logFriChannelInfo() { + FRIChannelInformation info = _friSession.getFRIChannelInformation(); + getLogger().info("[FRI] Состояние : " + info.getFRISessionState()); + getLogger().info("[FRI] Качество : " + info.getQuality()); + getLogger().info("[FRI] Jitter : " + info.getJitter() + " мс"); + getLogger().info("[FRI] Latency : " + info.getLatency() + " мс"); + } + +} diff --git a/src/iiwa_sunrise/src/TeachKuka.java b/src/iiwa_sunrise/src/TeachKuka.java new file mode 100644 index 0000000..8f44a16 --- /dev/null +++ b/src/iiwa_sunrise/src/TeachKuka.java @@ -0,0 +1,601 @@ +package application; + +//══════════════════════════════════════════════════════════════════════ +// +// +//TeachKuka.java +//Sunrise OS 1.16 | Servoing 1.16 | ApplicationFramework 1.2 +// +// +//Режимы работы: +// Режим 1: Захват позиции +// Режим 2: Запись и воспроизведение траектории +// +// +//══════════════════════════════════════════════════════════════════════ + +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.atomic.AtomicBoolean; + +import javax.inject.Inject; +import javax.inject.Named; + +import com.kuka.connectivity.motionModel.smartServo.ISmartServoRuntime; +import com.kuka.connectivity.motionModel.smartServo.SmartServo; +import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; +import com.kuka.roboticsAPI.controllerModel.Controller; +import com.kuka.roboticsAPI.deviceModel.JointPosition; +import com.kuka.roboticsAPI.deviceModel.LBR; +import com.kuka.roboticsAPI.geometricModel.CartDOF; +import com.kuka.roboticsAPI.geometricModel.Frame; +import com.kuka.roboticsAPI.geometricModel.Tool; +import com.kuka.roboticsAPI.motionModel.BasicMotions; +import com.kuka.roboticsAPI.motionModel.IMotionContainer; +import com.kuka.roboticsAPI.motionModel.controlModeModel.CartesianImpedanceControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; +import com.kuka.roboticsAPI.uiModel.ApplicationDialogType; + +public class TeachKuka extends RoboticsAPIApplication { + + @Inject + private LBR _lbr; + + @Inject + private Controller _lbrController; + + @Inject + @Named("tool1") + private Tool _gripper; + +// ==== КОНСТАНТНЫ ===== + private static final double[] HOME_POSITION_RAD = { + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + }; + + private static final double[] WORKING_POSITION_RAD = { + 0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0 + }; + + private static final double HOME_BLENDING_REL = 0.5; + private static final double APPROACH_VELOCITY_REL = 0.3; + private static final double REPLAY_VELOCITY_REL = 0.2; + + private static final double GRAV_COMP_STIFFNESS_TRANSL = 5.0; // N/m + private static final double GRAV_COMP_STIFFNESS_ROT = 1.0; // Nm/рад + private static final double GRAV_COMP_DAMPING = 0.7; + + private static final long GRAV_COMP_UPDATE_INTERVAL_MS = 10L; + + // Интервал записи точек (мс). 100 мс = 10 точек/сек + private static final long RECORD_INTERVAL_MS = 100L; + // Лимит точек: 3000 * 100 мс = 5 минут записи + private static final int MAX_TRAJECTORY_POINTS = 3000; + // Задержка перед стартом воспроизведения — чтобы успеть отойти + private static final long PRE_REPLAY_DELAY_MS = 2000L; +// Порог детекции столкновения по внешнему суставному моменту (Нм) +// 4–6 Нм - высокая чувствительность +// 7–10 Нм - средняя (производственная среда) + private static final double COLLISION_TORQUE_THRESHOLD_NM = 6.0; + // Период опроса пока путь заблокирован (мс) + private static final long OBSTACLE_POLL_INTERVAL_MS = 100L; + +// ==== СОСТОЯНИЕ ПРИЛОЖЕНИЯ + private final List recordedTrajectory = + new ArrayList(); + private final AtomicBoolean stopRecordingFlag = + new AtomicBoolean(false); + private final AtomicBoolean stopGravCompFlag = + new AtomicBoolean(false); + private volatile IMotionContainer activeMotionContainer = null; + private volatile ISmartServoRuntime gravCompRuntime = null; + + private Thread recordingThread = null; + private Thread gravCompThread = null; + + @Override + public void initialize() { + _gripper.attachTo(_lbr.getFlange()); + + getLogger().info(" Программа : TeachKuka.java"); + getLogger().info(" Sunrise OS 1.16 | Servoing 1.16"); + getLogger().info(" Робот : " + _lbr.getName()); + getLogger().info(" Суставов : " + _lbr.getJointCount()); + getLogger().info(" Контроллер: " + _lbrController.getName()); + getLogger().info(" Захват: " + _gripper.getName() + " -> прикреплён к фланцу"); + + } + + @Override + public void run() throws Exception { + + moveToHomePosition(); + + boolean running = true; + + while (running) { + int choice = showMainMenu(); + + switch (choice) { + case 0: + positionSnapshot(); + break; + case 1: + trajectoryRecord(); + break; + case 2: + default: + running = false; + break; + } + } + + getLogger().info("Выход. Возвращаюсь в Home..."); + moveToHomePosition(); + getLogger().info("Программа завершена."); + } + + @Override + public void dispose() { + stopRecordingFlag.set(true); + stopGravCompFlag.set(true); + stopGravCompThread(); + stopRecordingThread(); + cancelActiveMotion(); + super.dispose(); + } + + private int showMainMenu() { + return getApplicationUI().displayModalDialog( + ApplicationDialogType.QUESTION, + "Выберите режим работы", + "Режим 1: Позиция", + "Режим 2: Траектория", + "Выход" + ); + } + +// Режим 1 + private void positionSnapshot() throws Exception { + getLogger().info("Вход в Режим 1: Захват позиции"); + + moveToWorkingPosition(); + validateLoadModel(); + + startGravCompOnTool(); + + getLogger().info("Ведите робота рукой. Он останется там где вы его поставите."); + + boolean inMode = true; + while (inMode) { + int choice = getApplicationUI().displayModalDialog( + ApplicationDialogType.INFORMATION, + "Выбери действие", + "Получить позицию", + "Назад" + ); + + if (choice == 0) { + logCurrentPosition(); + } else { + inMode = false; + } + } + + stopGravComp(); + getLogger().info("Режим 1 завершён. Тормоз активирован."); + } + +// Режим 2 + private void trajectoryRecord() throws Exception { + getLogger().info("Вход в Режим 2: Запись траектории"); + + moveToWorkingPosition(); + validateLoadModel(); + + synchronized (recordedTrajectory) { + recordedTrajectory.clear(); + } + + startGravCompOnTool(); + startRecordingThread(); + + boolean inMode = true; + while (inMode) { + + int choise = getApplicationUI().displayModalDialog( + ApplicationDialogType.INFORMATION, + "Выберите действие", + + "Повторить траекторию", + "Рестарт", + "Назад" + ); + + if (choise == 0) { + stopRecordingThread(); + stopGravComp(); + + List snapshot; + synchronized (recordedTrajectory) { + snapshot = new ArrayList(recordedTrajectory); + } + + if (snapshot.size() < 2) { + getLogger().warn("Траектория слишком короткая (< 2 точек)." + + " Переместите робота и попробуйте снова."); + } else { + replayTrajectory(snapshot); + } + getLogger().info("Воспроизведение закончено. Начинаю новую запись..."); + synchronized (recordedTrajectory) { + recordedTrajectory.clear(); + } + validateLoadModel(); + startGravCompOnTool(); + startRecordingThread(); + } else if (choise == 1) { + stopRecordingThread(); + stopGravComp(); + synchronized (recordedTrajectory) { + recordedTrajectory.clear(); + } + + getLogger().info("Запись сброшена. Начинаю заново..."); + validateLoadModel(); + startGravCompOnTool(); + startRecordingThread(); + } else { + stopRecordingThread(); + stopGravComp(); + inMode = false; + } + } + + getLogger().info("Режим 2 завершён. Тормоз активирован."); + } + +// ==== Вспомогательные методоы ==== +// Запись данных + private void startRecordingThread() { + stopRecordingFlag.set(false); + + recordingThread = new Thread(new Runnable() { + @Override + public void run() { + recordingLoop(); + } + }, "TrajectoryRecorder"); + + recordingThread.setDaemon(true); + recordingThread.start(); + + getLogger().info("[Запись] Запущена. Интервал: " + + RECORD_INTERVAL_MS + " мс."); + } + + private void recordingLoop() { + getLogger().info("[Запись] Поток запущен."); + + while (!stopRecordingFlag.get()) { + try { + JointPosition pose = _lbr.getCurrentJointPosition(); + + synchronized (recordedTrajectory) { + if (recordedTrajectory.size() >= MAX_TRAJECTORY_POINTS) { + getLogger().warn("[Запись] Лимит " + + MAX_TRAJECTORY_POINTS + " точек достигнут." + + " Запись остановлена."); + break; + } + recordedTrajectory.add(pose); + } + + Thread.sleep(RECORD_INTERVAL_MS); + + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + break; + } catch (Exception e) { + getLogger().error("[Запись] Ошибка: " + e.getMessage()); + break; + } + } + + getLogger().info("[Запись] Поток остановлен. Точек: " + + recordedTrajectory.size()); + } + + private void stopRecordingThread() { + stopRecordingFlag.set(true); + if (recordingThread != null && recordingThread.isAlive()) { + recordingThread.interrupt(); + try { + recordingThread.join(2000); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + recordingThread = null; + } + } + +// Воспроизведение траекторий + private void replayTrajectory(List trajectory) throws Exception { + int total = trajectory.size(); + + getLogger().info("==========================================="); + getLogger().info(" ВОСПРОИЗВЕДЕНИЕ | Точек: " + total + + " | Скорость: " + (int)(REPLAY_VELOCITY_REL * 100) + "%"); + getLogger().info(" Старт через " + (PRE_REPLAY_DELAY_MS / 1000) + + " сек. Отойдите от робота!"); + getLogger().info("==========================================="); + + Thread.sleep(PRE_REPLAY_DELAY_MS); + + getLogger().info("Перемещаюсь к начальной точке..."); + _lbr.move( + BasicMotions.ptp(trajectory.get(0)) + .setJointVelocityRel(REPLAY_VELOCITY_REL) + ); + getLogger().info("Начальная точка достигнута."); + + SmartServo replayServo = new SmartServo(trajectory.get(0)); + replayServo.setMinimumTrajectoryExecutionTime(8e-3); + replayServo.setTimeoutAfterGoalReach(3600); + replayServo.setJointVelocityRel(REPLAY_VELOCITY_REL); + + _gripper.getDefaultMotionFrame() + .moveAsync(replayServo.setMode(new PositionControlMode())); + + ISmartServoRuntime replayRuntime = replayServo.getRuntime(); + getLogger().info("SmartServo активирован. Воспроизведение..."); + + int pauseCount = 0; + long totalWaitMs = 0L; + + for (int i = 1; i < total; i++) { + + if (isObstacleDetected(replayRuntime)) { + pauseCount++; + int waitCount = 0; + getLogger().warn("ПРЕПЯТСТВИЕ на точке " + i + "/" + total + + ". Удерживаю позицию..."); + + while (isObstacleDetected(replayRuntime)) { + replayRuntime.setDestination(_lbr.getCurrentJointPosition()); + Thread.sleep(OBSTACLE_POLL_INTERVAL_MS); + waitCount++; + totalWaitMs += OBSTACLE_POLL_INTERVAL_MS; + if (waitCount % 30 == 0) { + getLogger().warn(" Ожидание: " + + (waitCount * OBSTACLE_POLL_INTERVAL_MS / 1000) + + " сек. Освободите путь."); + } + } + getLogger().info("Путь свободен. Продолжаю с точки " + + i + "/" + total); + } + + replayRuntime.setDestination(trajectory.get(i)); + Thread.sleep(RECORD_INTERVAL_MS); + } + + // stopMotion ПОСЛЕ цикла, не внутри него + replayRuntime.stopMotion(); + + getLogger().info("==========================================="); + getLogger().info(" ГОТОВО | Точек: " + total + + " | Пауз: " + pauseCount + + (pauseCount > 0 + ? " | Ожидание: " + (totalWaitMs / 1000.0) + " сек" + : "")); + getLogger().info("==========================================="); + } + +// Проверка наличия контакта с препятсвием + private boolean isObstacleDetected(ISmartServoRuntime runtime) { + try { + + double[] tauExt = runtime.getAxisTauExtMsr(); + if (tauExt == null) { + return false; + } + for (int j = 0; j < tauExt.length; j++) { + if (Math.abs(tauExt[j]) > COLLISION_TORQUE_THRESHOLD_NM) { + getLogger().info(String.format( + " [Детекция] J%d: tau_ext=%.2f Нм (порог %.1f Нм)", + j + 1, tauExt[j], COLLISION_TORQUE_THRESHOLD_NM)); + return true; + } + } + } catch (Exception e) { + getLogger().warn("[Детекция] Ошибка: " + e.getMessage()); + } + + return false; + } + +// Компенсация массы робота + private void startGravCompOnTool() throws InterruptedException { + CartesianImpedanceControlMode gravComp = createGravityCompMode(); + + SmartServo smartServo = new SmartServo(_lbr.getCurrentJointPosition()); + smartServo.setMinimumTrajectoryExecutionTime(8e-3); + smartServo.setTimeoutAfterGoalReach(3600); + smartServo.setJointVelocityRel(0.5); + + activeMotionContainer = _gripper + .getDefaultMotionFrame() + .moveAsync(smartServo.setMode(gravComp)); + gravCompRuntime = smartServo.getRuntime(); + + stopGravCompFlag.set(false); + gravCompThread = new Thread(new Runnable() { + @Override + public void run() { + gravCompAnchorLoop(); + } + }, "GravCompAnchorUpdater"); + gravCompThread.setDaemon(true); + gravCompThread.start(); + + getLogger().info("Гравкомпенсация активна. Якорь следует за роботом."); + } + + private void gravCompAnchorLoop() { + getLogger().info("[GravComp] Поток обновления якоря запущен."); + + while (!stopGravCompFlag.get()) { + try { + ISmartServoRuntime rt = gravCompRuntime; + if (rt != null) { + rt.setDestination(_lbr.getCurrentJointPosition()); + } + Thread.sleep(GRAV_COMP_UPDATE_INTERVAL_MS); + + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + break; + } catch (Exception e) { + getLogger().warn("[GravComp] Ошибка обновления: " + e.getMessage()); + break; + } + } + + getLogger().info("[GravComp] Поток обновления остановлен."); + } + + private void stopGravComp() { + stopGravCompThread(); + + ISmartServoRuntime rt = gravCompRuntime; + if (rt != null) { + try { + rt.stopMotion(); + getLogger().info("SmartServo остановлен. Тормоз активирован."); + } catch (Exception e) { + getLogger().warn("Ошибка stopMotion(): " + e.getMessage()); + cancelActiveMotion(); + } finally { + gravCompRuntime = null; + activeMotionContainer = null; + } + } + } + + private void stopGravCompThread() { + stopGravCompFlag.set(true); + if (gravCompThread != null && gravCompThread.isAlive()) { + gravCompThread.interrupt(); + try { + gravCompThread.join(1000); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + gravCompThread = null; + } + } + +// Утилиты + private void logCurrentPosition() { + getLogger().info("==========================================="); + + Frame flangeInWorld = _lbr.getCurrentCartesianPosition(_lbr.getFlange()); + + double x_mm = flangeInWorld.getX(); + double y_mm = flangeInWorld.getY(); + double z_mm = flangeInWorld.getZ(); + double a_deg = Math.toDegrees(flangeInWorld.getAlphaRad()); + double b_deg = Math.toDegrees(flangeInWorld.getBetaRad()); + double c_deg = Math.toDegrees(flangeInWorld.getGammaRad()); + + getLogger().info(String.format(" XYZ [мм]: X=%9.3f Y=%9.3f Z=%9.3f", x_mm, y_mm, z_mm)); + getLogger().info(String.format(" ABC [мм]: [ ° ]: A=%9.3f B=%9.3f C=%9.3f", a_deg, b_deg, c_deg)); + + JointPosition joints = _lbr.getCurrentJointPosition(); + int n = _lbr.getJointCount(); + + StringBuilder rowDeg = new StringBuilder(" Суставы [ ° ]: "); + StringBuilder rowRad = new StringBuilder(" Суставы [рад]: "); + + for (int i=0; i Object Templates -> " + + _gripper.getName() + " -> Load data."); + getLogger().warn("Гравкомпенсация может работать некорректно."); + } + } + + /** + * Перемещает в Home (все оси = 0 + */ + private void moveToHomePosition() { + getLogger().info("Перемещение в Home..."); + _lbr.move( + BasicMotions.ptp(HOME_POSITION_RAD) + .setJointVelocityRel(APPROACH_VELOCITY_REL) + ); + getLogger().info("Home достигнута."); + } + +// Перемещает в рабочую позицию через Home + private void moveToWorkingPosition() throws Exception { + getLogger().info("Перемещение в рабочую позицию ..."); + _lbr.move( + BasicMotions.batch( + BasicMotions.ptp(HOME_POSITION_RAD) + .setBlendingRel(HOME_BLENDING_REL), + BasicMotions.ptp(WORKING_POSITION_RAD) + ).setJointVelocityRel(APPROACH_VELOCITY_REL) + ); + + getLogger().info("Рабочая позиция достигнута"); + } + + private void cancelActiveMotion() { + if (activeMotionContainer != null) { + try { + activeMotionContainer.cancel(); + getLogger().info("Движение отменено. Тормоз активирован."); + } catch (Exception e) { + getLogger().warn("Ошибка при cancel(): " + e.getMessage()); + } finally { + activeMotionContainer = null; + } + } + } + + +} \ No newline at end of file