grabardm/lightweight-cobot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

Update logic work system

Browse Source
This commit is contained in:
Даниил Грабарь
2026-05-25 07:38:07 +03:00
parent fff46f0ade
commit 2f079df087
1 changed files with 225 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/iiwa_sunrise/src/ServerFriRos2.java
+225 -198
View File
@@ -1,19 +1,30 @@
package ros; package ros;
// Sunrise OS 1.16 | FRI 1.16 | KUKA iiwa 7 /**
// * FRI bridge between KUKA iiwa 7 and ROS 2 via lbr_ros2_control.
// Режимы команды FRI: * FRI-мост между KUKA iiwa 7 и ROS 2 через lbr_ros2_control.
// POSITION - ROS2 задаёт целевые углы суставов *
// TORQUE - ROS2 задаёт добавочные моменты суставов * Tested on: Sunrise OS 1.16 / FRI 1.16 / iiwa 7 R800
// NO_COMMAND_MODE - FRI только читает состояние, ведение рукой * Проверено на: Sunrise OS 1.16 / FRI 1.16 / iiwa 7 R800
// *
// Режимы управления Sunrise (только для POSITION и TORQUE): * Two network interfaces are supported:
// POSITION_CONTROL - жёсткое позиционирование * Поддерживаются два сетевых интерфейса:
// JOINT_IMPEDANCE_CONTROL - упругое позиционирование с заданной жёсткостью *
// * KONI (X66) — dedicated high-speed FRI network, recommended.
// Сетевые интерфейсы: * Recommended send period: 5–10 ms.
// KONI - 192.170.10.10 (рекомендуется) * Поддерживает все режимы включая Monitor (ведение рукой).
// KLI - 192.168.21.31 *
* KLI (X6) — shared KRC control network, fallback option.
* Send period fixed at 10 ms to avoid packet loss on a shared bus.
* Monitor mode not available: KLI latency is too high for gravity
* compensation to be safe without a dedicated FRI stream.
* KLI latency слишком высока для безопасной гравкомпенсации.
*
* Control modes / Режимы управления:
* Position
* JointImpedance
* Monitor
*/
import java.util.concurrent.TimeUnit; import java.util.concurrent.TimeUnit;
import java.util.concurrent.TimeoutException; import java.util.concurrent.TimeoutException;
@@ -42,46 +53,71 @@ import com.kuka.roboticsAPI.uiModel.ApplicationDialogType;
public class ServerFriRos2 extends RoboticsAPIApplication { public class ServerFriRos2 extends RoboticsAPIApplication {
// Позиции для движения перед запуском FRI // CONFIGURE BEFORE DEPLOYMENT — проверить перед запуском на новом стенде
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};
// Сетевые адреса // IP address of the PC running the ROS 2 FRI node, as seen from the robot.
private static final String KONI_IP = "192.170.10.10"; // IP-адрес ПК с ROS 2 FRI-узлом со стороны робота.
private static final String KLI_IP = "192.168.21.31"; // KONI (X66 connector): default subnet is 192.170.10.x — change the last octet to match your PC.
// KLI (X6 connector): depends on your KRC network config.
private static final String KONI_IP = "192.170.10.10"; // <<< CHANGE THIS / ИЗМЕНИТЬ
private static final String KLI_IP = "192.168.21.31"; // <<< CHANGE THIS / ИЗМЕНИТЬ
private static final int FRI_CONNECT_TIMEOUT_SEC = 30; // Tool name as defined in Sunrise Workbench -> Object Templates.
// Имя инструмента из Sunrise Workbench -> Object Templates.
// Must have valid Load Data (mass, CoM, inertia) for Monitor mode gravity compensation.
// Для Monitor режима обязательно заполните Load Data (масса, ЦМ, инерция).
// @Named is set below on the _tool field
// Safe joint-space pose the robot moves to before FRI starts.
// Безопасная поза (в пространстве суставов) куда робот едет перед запуском FRI.
// Adjust to avoid collisions with your cell layout / инструментом / оснасткой.
private static final double[] ZERO_POSITION = {0, 0, 0, 0, 0, 0, 0}; // <<< CHECK / ПРОВЕРИТЬ
private static final double[] MONITOR_WORKING_POSITION = {0, 0, 0, -1.57, 0, 1.57, 0}; // <<< CHECK / ПРОВЕРИТЬ
// TUNING — fine-tune if needed / настройки при необходимости
// How long to wait for the ROS 2 client to connect before giving up.
// Время ожидания подключения FRI-клиента до отмены.
private static final int FRI_CONNECT_TIMEOUT_SEC = 30;
// Relative joint velocity used for approach moves (0.0–1.0 of rated speed).
// Относительная скорость для подъездных движений (0.0–1.0 от номинальной).
private static final double APPROACH_VEL = 0.30; private static final double APPROACH_VEL = 0.30;
// Параметры для NO_COMMAND_MODE: нулевая жёсткость позволяет свободно вести робота рукой // Stiffness/damping for Monitor (gravity-comp, zero-stiffness hand guiding).
// Жёсткость/демпфирование для Monitor: нулевая жёсткость = робот не сопротивляется руке.
private static final double MONITOR_JOINT_STIFFNESS = 0.0; private static final double MONITOR_JOINT_STIFFNESS = 0.0;
private static final double MONITOR_JOINT_DAMPING = 0.7; private static final double MONITOR_JOINT_DAMPING = 0.7;
// Режим команды FRI - что именно отправляет ROS2 в каждом цикле
private enum CommandMode { private enum CommandMode {
POSITION, POSITION,
TORQUE,
NO_COMMAND_MODE NO_COMMAND_MODE
} }
// Режим управления Sunrise - как контроллер обрабатывает команды
private enum ControlMode { private enum ControlMode {
POSITION_CONTROL, POSITION_CONTROL,
JOINT_IMPEDANCE_CONTROL JOINT_IMPEDANCE_CONTROL
} }
// Сетевой интерфейс для подключения FRI /**
* Encapsulates a network interface choice together with the IP address
* that will be passed to FRIConfiguration.
* Хранит выбор сетевого интерфейса и соответствующий IP для FRIConfiguration.
*
* The label is built from the IP constant so the dialog button always
* reflects the actual address without manual string maintenance.
* Label строится из IP-константы - при изменении IP адрес в кнопке обновится сам.
*/
private enum NetworkInterface { private enum NetworkInterface {
KONI("KONI (192.170.10.10) - выделенная FRI-сеть", KONI_IP), KONI(KONI_IP),
KLI("KLI (192.168.21.31) - основная сеть KRC", KLI_IP); KLI (KLI_IP);
final String label; final String label;
final String ip; final String ip;
NetworkInterface(String label, String ip) { NetworkInterface(String ip) {
this.label = label;
this.ip = ip; this.ip = ip;
this.label = name() + " — " + ip;
} }
} }
@@ -89,10 +125,10 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
private LBR _lbr; private LBR _lbr;
private Controller _lbrController; private Controller _lbrController;
// Инструмент из Sunrise WB - Object Templates tool1 // Must match the Object Template name in Sunrise Workbench.
// Масса и CoM берутся из Load Data этого объекта // Должно совпадать с именем объекта в Sunrise Workbench -> Object Templates.
@Inject @Inject
@Named("tool1") @Named("tool1") // <<< CHANGE THIS / ИЗМЕНИТЬ
private Tool _tool; private Tool _tool;
private NetworkInterface _selectedNetwork; private NetworkInterface _selectedNetwork;
@@ -113,10 +149,11 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
_lbrController = (Controller) getContext().getControllers().toArray()[0]; _lbrController = (Controller) getContext().getControllers().toArray()[0];
_lbr = (LBR) _lbrController.getDevices().toArray()[0]; _lbr = (LBR) _lbrController.getDevices().toArray()[0];
// Прикрепляем инструмент к фланцу для корректной гравкомпенсации // Attach tool so Sunrise accounts for its mass in all motion planning.
// Крепим инструмент, чтобы Sunrise учитывал его массу при всех движениях.
_tool.attachTo(_lbr.getFlange()); _tool.attachTo(_lbr.getFlange());
getLogger().info("ServerFriRos2 | KUKA iiwa 7 + ROS2 FRI"); getLogger().info("ServerFriRos2 | KUKA iiwa 7 | ROS2 FRI");
getLogger().info("Sunrise OS 1.16 | FRI 1.16"); getLogger().info("Sunrise OS 1.16 | FRI 1.16");
getLogger().info("Робот: " + _lbr.getName()); getLogger().info("Робот: " + _lbr.getName());
getLogger().info("Инструмент: " + _tool.getName()); getLogger().info("Инструмент: " + _tool.getName());
@@ -131,9 +168,8 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
moveToInitialPosition(); moveToInitialPosition();
switch (_selectedCommandMode) { switch (_selectedCommandMode) {
case POSITION: runPositionMode(); break; case POSITION: runPositionMode(); break;
case TORQUE: runTorqueMode(); break; case NO_COMMAND_MODE: runMonitorMode(); break;
case NO_COMMAND_MODE: runMonitorMode(); break;
} }
getLogger().info("Программа завершена."); getLogger().info("Программа завершена.");
@@ -142,6 +178,8 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
@Override @Override
public void dispose() { public void dispose() {
// Sunrise calls dispose() even if run() threw - make sure the session is always released.
// Sunrise вызывает dispose() даже при исключении в run() - сессия должна быть освобождена.
if (_friSession != null) { if (_friSession != null) {
getLogger().info("Закрытие FRI-сессии..."); getLogger().info("Закрытие FRI-сессии...");
try { try {
@@ -156,153 +194,142 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
} }
// Последовательный опрос конфигурации - каждый шаг зависит от предыдущего
private void requestUserConfig() { private void requestUserConfig() {
// Шаг 1: сетевой интерфейс
int netChoice = getApplicationUI().displayModalDialog( int netChoice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION, ApplicationDialogType.QUESTION,
"Шаг 1 - Сетевой интерфейс FRI\n\n" "Шаг 1 — Сетевой интерфейс FRI",
+ "KONI: выделенная высокоскоростная сеть (рекомендуется)\n"
+ "KLI: основная сеть KRC",
NetworkInterface.KONI.label, NetworkInterface.KONI.label,
NetworkInterface.KLI.label NetworkInterface.KLI.label
); );
_selectedNetwork = (netChoice == 0) ? NetworkInterface.KONI : NetworkInterface.KLI; _selectedNetwork = (netChoice == 0) ? NetworkInterface.KONI : NetworkInterface.KLI;
getLogger().info("Сетевой интерфейс: " + _selectedNetwork.label); getLogger().info("Сетевой интерфейс: " + _selectedNetwork.label);
// Шаг 2: режим команды FRI if (_selectedNetwork == NetworkInterface.KLI) {
int modeChoice = getApplicationUI().displayModalDialog( configureKli();
ApplicationDialogType.QUESTION,
"Шаг 2 - Режим команды FRI\n\n"
+ "Position: ROS2 задаёт угловые позиции суставов\n"
+ "Torque: ROS2 задаёт добавочные моменты суставов\n"
+ "Monitor: только чтение, ведение рукой (NO_COMMAND_MODE)",
"Position",
"Torque",
"Monitor"
);
if (modeChoice == 0) {
_selectedCommandMode = CommandMode.POSITION;
selectControlMode();
selectSendPeriodForPosition();
} else if (modeChoice == 1) {
_selectedCommandMode = CommandMode.TORQUE;
// TORQUE всегда требует JointImpedanceControlMode на стороне Sunrise
_selectedControlMode = ControlMode.JOINT_IMPEDANCE_CONTROL;
selectJointStiffness();
selectSendPeriodForTorque();
} else { } else {
_selectedCommandMode = CommandMode.NO_COMMAND_MODE; configureKoni();
// Нулевая жёсткость задана константой, пользователю выбирать нечего
_selectedControlMode = ControlMode.JOINT_IMPEDANCE_CONTROL;
_sendPeriodMs = 2;
_jointStiffness = 0.0;
getLogger().info("Monitor (NO_COMMAND_MODE): период = " + _sendPeriodMs + " мс");
} }
logConfiguration(); logConfiguration();
} }
// Шаг 3a (только для POSITION): выбор режима управления Sunrise // KLI: Position and JointImpedance only; send period fixed at 10 ms.
private void selectControlMode() { // KLI: доступны только Position и JointImpedance; период зафиксирован на 10 мс.
// Monitor is excluded because KLI latency makes zero-stiffness guiding unsafe.
// Monitor исключён: задержки KLI делают ведение рукой с нулевой жёсткостью небезопасным.
private void configureKli() {
int ctrlChoice = getApplicationUI().displayModalDialog( int modeChoice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION, ApplicationDialogType.QUESTION,
"Шаг 3 - Режим управления Sunrise (Position)\n\n" "Шаг 2 — Режим управления",
+ "PositionControl: жёсткое позиционирование, максимальная точность следования\n" "Position",
+ "JointImpedance: упругое позиционирование, задаётся жёсткость суставов",
"PositionControl",
"JointImpedance" "JointImpedance"
); );
if (ctrlChoice == 0) { _selectedCommandMode = CommandMode.POSITION;
if (modeChoice == 0) {
_selectedControlMode = ControlMode.POSITION_CONTROL; _selectedControlMode = ControlMode.POSITION_CONTROL;
_jointStiffness = 0.0; _jointStiffness = 0.0;
getLogger().info("Режим управления: PositionControlMode");
} else { } else {
_selectedControlMode = ControlMode.JOINT_IMPEDANCE_CONTROL; _selectedControlMode = ControlMode.JOINT_IMPEDANCE_CONTROL;
selectJointStiffness(); selectJointStiffness();
} }
// KLI shared bus cannot reliably sustain 5 ms cycles; 10 ms is the safe floor.
// Общая шина KLI не выдерживает стабильные циклы 5 мс; 10 мс — безопасный минимум.
_sendPeriodMs = 10;
getLogger().info("KLI: период зафиксирован 10 мс");
}
// KONI: all three modes available; operator picks the send period.
// KONI: доступны все три режима; оператор выбирает период отправки.
private void configureKoni() {
int modeChoice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION,
"Шаг 2 — Режим управления",
"Position",
"JointImpedance",
"Monitor"
);
if (modeChoice == 0) {
_selectedCommandMode = CommandMode.POSITION;
_selectedControlMode = ControlMode.POSITION_CONTROL;
_jointStiffness = 0.0;
selectSendPeriod();
} else if (modeChoice == 1) {
_selectedCommandMode = CommandMode.POSITION;
_selectedControlMode = ControlMode.JOINT_IMPEDANCE_CONTROL;
selectJointStiffness();
selectSendPeriod();
} else {
_selectedCommandMode = CommandMode.NO_COMMAND_MODE;
_selectedControlMode = ControlMode.JOINT_IMPEDANCE_CONTROL;
// Monitor doesn't drive joints, so 2 ms gives the densest state stream to ROS 2.
// Monitor не командует суставами, поэтому 2 мс дают максимально плотный поток в ROS 2.
_sendPeriodMs = 2;
_jointStiffness = 0.0;
getLogger().info("Monitor: период = " + _sendPeriodMs + " мс");
}
} }
// Выбор жёсткости суставов для JointImpedanceControlMode
private void selectJointStiffness() { private void selectJointStiffness() {
int sChoice = getApplicationUI().displayModalDialog( int sChoice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION, ApplicationDialogType.QUESTION,
"Жёсткость суставов [Нм/рад]\n\n" "Жёсткость суставов [Нм/рад]",
+ "Высокая жёсткость: точное следование, меньше отклонение от траектории\n" "1500",
+ "Низкая жёсткость: мягкое взаимодействие со средой\n\n" "1000",
+ "Внимание: 1500 Нм/рад только в производственном режиме без людей в зоне!", "800",
"1500 - жёсткий / производственный", "500"
"1000 - стандарт",
"800 - средний",
"500 - мягкий / взаимодействие",
"300 - очень мягкий"
); );
_jointStiffness = new double[]{1500, 1000, 800, 500, 300}[sChoice]; _jointStiffness = new double[]{1500, 1000, 800, 500}[sChoice];
getLogger().info("Жёсткость суставов: " + _jointStiffness + " Нм/рад"); getLogger().info("Жёсткость суставов: " + _jointStiffness + " Нм/рад");
} }
private void selectSendPeriodForPosition() { private void selectSendPeriod() {
int pChoice = getApplicationUI().displayModalDialog( int pChoice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION, ApplicationDialogType.QUESTION,
"Шаг 4 - Период отправки FRI [мс] (Position)\n\n" "Шаг 3 — Период отправки FRI",
+ "10 мс: стабильно, подходит для большинства сетей\n"
+ "5 мс: стандарт для ros2_control на 200 Гц\n"
+ "2 мс: быстро, требует низкого джиттера сети",
"10 мс", "10 мс",
"5 мс", "5 мс"
"2 мс"
); );
_sendPeriodMs = new int[]{10, 5, 2}[pChoice]; _sendPeriodMs = (pChoice == 0) ? 10 : 5;
getLogger().info("Период отправки: " + _sendPeriodMs + " мс");
}
private void selectSendPeriodForTorque() {
int pChoice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION,
"Шаг 4 - Период отправки FRI [мс] (Torque)\n\n"
+ "При пропуске пакета Sunrise автоматически переходит в PositionHold.\n"
+ "Рекомендуется 1-2 мс при стабильной KONI-сети.",
"5 мс",
"2 мс (рекомендуется)",
"1 мс (максимальная частота)"
);
_sendPeriodMs = new int[]{5, 2, 1}[pChoice];
getLogger().info("Период отправки: " + _sendPeriodMs + " мс"); getLogger().info("Период отправки: " + _sendPeriodMs + " мс");
} }
private void logConfiguration() { private void logConfiguration() {
getLogger().info("Итоговая конфигурация:"); getLogger().info("── Конфигурация ──────────────────");
getLogger().info(" Сеть: " + _selectedNetwork.label); getLogger().info(" Network : " + _selectedNetwork.label);
getLogger().info(" IP: " + _selectedNetwork.ip); getLogger().info(" FRI mode : " + _selectedCommandMode.name());
getLogger().info(" Режим команды FRI: " + _selectedCommandMode.name()); getLogger().info(" Ctrl mode : " + _selectedControlMode.name());
getLogger().info(" Режим управления Sunrise: " + _selectedControlMode.name()); getLogger().info(" Period : " + _sendPeriodMs + " мс");
getLogger().info(" Период отправки: " + _sendPeriodMs + " мс"); getLogger().info(" Tool : " + _tool.getName());
getLogger().info(" Инструмент: " + _tool.getName());
if (_selectedControlMode == ControlMode.JOINT_IMPEDANCE_CONTROL && _jointStiffness > 0) { if (_selectedControlMode == ControlMode.JOINT_IMPEDANCE_CONTROL && _jointStiffness > 0) {
getLogger().info(" Жёсткость суставов: " + _jointStiffness + " Нм/рад"); getLogger().info(" Stiffness : " + _jointStiffness + " Нм/рад");
} }
getLogger().info("──────────────────────");
} }
private void moveToInitialPosition() { private void moveToInitialPosition() {
if (_selectedCommandMode == CommandMode.NO_COMMAND_MODE) { if (_selectedCommandMode == CommandMode.NO_COMMAND_MODE) {
getLogger().info("Движение в рабочую позицию Monitor (через нулевую)..."); // Move through zero first to avoid large single-joint swings.
// Сначала едем через ноль, чтобы не было больших движений по одному суставу.
getLogger().info("Движение в рабочую позицию Monitor...");
_lbr.move( _lbr.move(
BasicMotions.batch( BasicMotions.batch(
BasicMotions.ptp(ZERO_POSITION).setBlendingRel(0.5), BasicMotions.ptp(ZERO_POSITION).setBlendingRel(0.5),
@@ -332,61 +359,44 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
PositionHold posHold = new PositionHold(ctrlMode, -1, TimeUnit.SECONDS); PositionHold posHold = new PositionHold(ctrlMode, -1, TimeUnit.SECONDS);
getLogger().info("Position режим активен. Ожидаю команды от ROS2..."); getLogger().info("Position режим активен. Ожидаю команды от ROS2...");
_lbr.move(posHold.addMotionOverlay(_friOverlay)); try {
_lbr.move(posHold.addMotionOverlay(_friOverlay));
_friSession.close(); } catch (Exception e) {
_friSession = null; // Normal exit path when the ROS 2 client closes the FRI session.
getLogger().info("Position режим завершён. FRI закрыт."); // Штатный путь выхода при закрытии FRI-сессии со стороны ROS 2.
} getLogger().info("FRI сеанс закрыт.");
private void runTorqueMode() {
getLogger().info("Запуск Torque режима, жёсткость: " + _jointStiffness + " Нм/рад");
if (!setupFriSession(ClientCommandMode.TORQUE)) {
return;
} }
JointImpedanceControlMode ctrlMode = new JointImpedanceControlMode( closeFriSession();
_jointStiffness, _jointStiffness, _jointStiffness, getLogger().info("Position режим завершён.");
_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 закрыт.");
} }
private void runMonitorMode() { private void runMonitorMode() {
getLogger().info("Запуск Monitor режима (NO_COMMAND_MODE)."); getLogger().info("Запуск Monitor режима...");
// Фаза A: валидация Load Data инструмента из Sunrise WB // Validate tool Load Data before enabling zero-stiffness guiding —
// incorrect inertia will make gravity compensation fight the operator.
// Проверяем Load Data до включения нулевой жёсткости:
// некорректная инерция заставит гравкомпенсацию работать против оператора.
validateLoadModel(); validateLoadModel();
// Фаза B: ждём подтверждения оператора, что ROS2 FRI-узел запущен
getApplicationUI().displayModalDialog( getApplicationUI().displayModalDialog(
ApplicationDialogType.INFORMATION, ApplicationDialogType.INFORMATION,
"Запустите ROS2 FRI-узел на ПК (" + _selectedNetwork.ip + ").\n\n" "Запустите ROS2 FRI-узел на ПК (" + _selectedNetwork.ip + ").\n\n"
+ "Нажмите OK когда ros2_control_node активен.", + "Нажмите OK когда ros2_control_node активен.",
"OK - ROS2 готов" "OK — ROS2 готов"
); );
// Фаза C: FRI в режиме только чтения (NO_COMMAND_MODE)
if (!setupFriSession(ClientCommandMode.NO_COMMAND_MODE)) { if (!setupFriSession(ClientCommandMode.NO_COMMAND_MODE)) {
return; return;
} }
// Фаза D: PositionHold с нулевой жёсткостью - свободное ведение рукой // Zero stiffness + moderate damping = the robot doesn't resist hand guiding
// but also doesn't flop around. Gravity is compensated by the controller.
// Нулевая жёсткость + умеренное демпфирование: робот не сопротивляется руке,
// но и не болтается. Гравитация компенсируется контроллером.
JointImpedanceControlMode guidingMode = new JointImpedanceControlMode( 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, MONITOR_JOINT_STIFFNESS, MONITOR_JOINT_STIFFNESS, MONITOR_JOINT_STIFFNESS, MONITOR_JOINT_STIFFNESS, MONITOR_JOINT_STIFFNESS,
@@ -397,19 +407,33 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
PositionHold posHold = new PositionHold(guidingMode, -1, TimeUnit.SECONDS); PositionHold posHold = new PositionHold(guidingMode, -1, TimeUnit.SECONDS);
getLogger().info("Monitor режим активен."); getLogger().info("Monitor режим активен.");
getLogger().info("Ведите робота рукой - он не сопротивляется."); getLogger().info("Ведите робота рукой, команды будут транслироваться в ROS2.");
getLogger().info("Данные суставов транслируются в ROS2 каждые " + _sendPeriodMs + " мс."); getLogger().info("Данные суставов транслируются в ROS2 каждые " + _sendPeriodMs + " мс.");
getLogger().info("Остановите FRI-клиент для завершения."); getLogger().info("Остановите FRI-клиент для завершения.");
_lbr.move(posHold); try {
_lbr.move(posHold);
} catch (Exception e) {
getLogger().info("FRI сеанс закрыт.");
}
_friSession.close(); closeFriSession();
_friSession = null; getLogger().info("Monitor режим завершён.");
getLogger().info("Monitor режим завершён. FRI закрыт."); }
// Silently closes the session — the client may have already torn it down.
// Тихо закрывает сессию — клиент мог уже закрыть её со своей стороны.
private void closeFriSession() {
if (_friSession != null) {
try {
_friSession.close();
} catch (Exception ignored) {}
_friSession = null;
}
} }
// Создаёт объект режима управления на основе выбора пользователя
private AbstractMotionControlMode buildControlMode() { private AbstractMotionControlMode buildControlMode() {
if (_selectedControlMode == ControlMode.POSITION_CONTROL) { if (_selectedControlMode == ControlMode.POSITION_CONTROL) {
@@ -417,7 +441,10 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
return new PositionControlMode(); return new PositionControlMode();
} }
// JointImpedanceControlMode - одинаковая жёсткость для всех суставов // Uniform stiffness across all joints is a reasonable default; tune per-joint
// if the task requires asymmetric compliance (e.g. soft wrist, stiff elbow).
// Одинаковая жёсткость по всем суставам — разумный старт; при необходимости
// настройте каждый сустав отдельно (напр. мягкое запястье, жёсткий локоть).
JointImpedanceControlMode mode = new JointImpedanceControlMode( JointImpedanceControlMode mode = new JointImpedanceControlMode(
_jointStiffness, _jointStiffness, _jointStiffness, _jointStiffness, _jointStiffness, _jointStiffness,
_jointStiffness, _jointStiffness, _jointStiffness, _jointStiffness, _jointStiffness, _jointStiffness,
@@ -429,8 +456,8 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
} }
// Проверяет Load Data инструмента из Sunrise WB через SmartServo.validateForImpedanceMode. // SmartServo.validateForImpedanceMode checks that mass/CoM/inertia are non-zero.
// Корректные данные нагрузки обязательны для точной гравкомпенсации в Monitor режиме. // SmartServo.validateForImpedanceMode проверяет, что масса/ЦМ/инерция заданы ненулевыми.
private void validateLoadModel() { private void validateLoadModel() {
getLogger().info("Валидация нагрузки инструмента: " + _tool.getName()); getLogger().info("Валидация нагрузки инструмента: " + _tool.getName());
@@ -440,27 +467,26 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
if (valid) { if (valid) {
getLogger().info("Load Data валидны. Гравкомпенсация будет работать точно."); getLogger().info("Load Data валидны. Гравкомпенсация будет работать точно.");
} else { } else {
getLogger().warn("Валидация Load Data не прошла для инструмента: " + _tool.getName()); getLogger().warn("Load Data не заданы для: " + _tool.getName());
getLogger().warn("Задайте данные в Sunrise WB -> Object Templates -> " getLogger().warn("Sunrise WB → Object Templates → " + _tool.getName() + " → Load Data");
+ _tool.getName() + " -> Load Data"); getLogger().warn("Требуются: Mass [кг], Centre of Mass [мм], Inertia [кг·м²]");
getLogger().warn("(Mass [кг], Centre of Mass [мм], Inertia [кг/м2])"); getLogger().warn("Без них гравкомпенсация в Monitor режиме будет неточной.");
getLogger().warn("Гравкомпенсация в Monitor режиме может работать некорректно.");
int choice = getApplicationUI().displayModalDialog( int choice = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION, ApplicationDialogType.QUESTION,
"Load Data инструмента '" + _tool.getName() + "' не заданы.\n\n" "Load Data инструмента '" + _tool.getName() + "' не заданы.\n\n"
+ "Без корректных данных нагрузки гравкомпенсация\n" + "Без них гравитационная компенсация работает некорректно.\n\n"
+ "будет работать с ошибкой.\n\n" + "Задайте данные:\n"
+ "Задайте данные в Sunrise WB -> Object Templates -> " + "Sunrise WB → Object Templates → " + _tool.getName() + " → Load Data\n"
+ _tool.getName() + " -> Load Data\nи перезапустите программу.\n\n" + "и перезапустите программу.\n\n"
+ "Или продолжите без корректной нагрузки (на свой риск).", + "Или продолжите на свой риск.",
"Продолжить", "Продолжить",
"Остановить" "Остановить"
); );
if (choice == 1) { if (choice == 1) {
throw new RuntimeException( throw new RuntimeException(
"Остановлено оператором: Load Data не заданы для " + _tool.getName()); "Остановлено: Load Data не заданы для " + _tool.getName());
} }
} }
} }
@@ -474,9 +500,9 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
getLogger().info("Создание FRI-сессии..."); getLogger().info("Создание FRI-сессии...");
getLogger().info("Хост: " + _friConfig.getHostName() getLogger().info("Хост: " + _friConfig.getHostName()
+ ", порт: " + _friConfig.getPortOnRemote()); + " | Порт: " + _friConfig.getPortOnRemote()
getLogger().info("Режим команды: " + commandMode.name()); + " | Режим: " + commandMode.name()
getLogger().info("Период отправки: " + _friConfig.getSendPeriodMilliSec() + " мс"); + " | Период: " + _friConfig.getSendPeriodMilliSec() + " мс");
_friSession = new FRISession(_friConfig); _friSession = new FRISession(_friConfig);
_friSession.addFRISessionListener(_friListener); _friSession.addFRISessionListener(_friListener);
@@ -488,8 +514,7 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
} catch (TimeoutException e) { } catch (TimeoutException e) {
getLogger().error("Таймаут FRI! Клиент не ответил за " + FRI_CONNECT_TIMEOUT_SEC + " с."); getLogger().error("Таймаут FRI! Клиент не ответил за " + FRI_CONNECT_TIMEOUT_SEC + " с.");
getLogger().error("Убедитесь, что ROS2 FRI-узел запущен на " + _selectedNetwork.ip); getLogger().error("Убедитесь, что ROS2 FRI-узел запущен на " + _selectedNetwork.ip);
_friSession.close(); closeFriSession();
_friSession = null;
return false; return false;
} }
@@ -500,8 +525,9 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
_friOverlay = new FRIJointOverlay(_friSession, commandMode); _friOverlay = new FRIJointOverlay(_friSession, commandMode);
getLogger().info("FRIJointOverlay создан для режима: " + commandMode.name()); getLogger().info("FRIJointOverlay создан для режима: " + commandMode.name());
} else { } else {
// In NO_COMMAND_MODE the robot state is streamed but no overlay is needed.
// В NO_COMMAND_MODE состояние транслируется, но overlay не нужен.
_friOverlay = null; _friOverlay = null;
getLogger().info("Monitor: FRIJointOverlay не создаётся (только чтение данных).");
} }
return true; return true;
@@ -513,26 +539,27 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
@Override @Override
public void onFRIConnectionQualityChanged(FRIChannelInformation info) { public void onFRIConnectionQualityChanged(FRIChannelInformation info) {
getLogger().info("FRI качество изменилось: " + info.getQuality() getLogger().info("FRI quality: " + info.getQuality()
+ ", jitter=" + info.getJitter() + " мс" + " | jitter=" + info.getJitter() + " мс"
+ ", latency=" + info.getLatency() + " мс"); + " | latency=" + info.getLatency() + " мс");
} }
@Override @Override
public void onFRISessionStateChanged(FRIChannelInformation info) { public void onFRISessionStateChanged(FRIChannelInformation info) {
getLogger().info("FRI состояние изменилось: " + info.getFRISessionState() getLogger().info("FRI state: " + info.getFRISessionState()
+ ", jitter=" + info.getJitter() + " мс" + " | jitter=" + info.getJitter() + " мс"
+ ", latency=" + info.getLatency() + " мс"); + " | latency=" + info.getLatency() + " мс");
} }
}; };
} }
private void logFriChannelInfo() { private void logFriChannelInfo() {
FRIChannelInformation info = _friSession.getFRIChannelInformation(); FRIChannelInformation info = _friSession.getFRIChannelInformation();
getLogger().info("FRI состояние: " + info.getFRISessionState()); getLogger().info("FRI state : " + info.getFRISessionState());
getLogger().info("FRI качество: " + info.getQuality()); getLogger().info("FRI quality : " + info.getQuality());
getLogger().info("FRI jitter: " + info.getJitter() + " мс"); getLogger().info("FRI jitter : " + info.getJitter() + " мс");
getLogger().info("FRI latency: " + info.getLatency() + " мс"); getLogger().info("FRI latency : " + info.getLatency() + " мс");
} }