feat: Enhance iiwa_description and iiwa_sunrise functionality
- Added a new link "tcp" and a fixed joint "patron_tcp" in patron.xacro for better tool control. - Updated ServerFriRos2.java to improve command mode handling and user configuration steps, including clearer logging and better control mode selection. - Removed the deprecated iiwa_ros2.java file to streamline the codebase. - Enhanced test_motion_sequence.py to allow dynamic topic recording based on user input, with improved logging for bag file management.
This commit is contained in:
Даниил Грабарь
@@ -63,12 +63,33 @@ ros2 service call /iiwa/move_to_named iiwa_msgs/srv/MoveToNamedPose \
|
|||||||
ros2 service call /iiwa/stop std_srvs/srv/Trigger "{}"
|
ros2 service call /iiwa/stop std_srvs/srv/Trigger "{}"
|
||||||
```
|
```
|
||||||
|
|
||||||
Пример сбора данных:
|
Примеры использования `test_motion_sequence`:
|
||||||
```bash
|
```bash
|
||||||
ros2 run iiwa_utils test_motion_sequence --ros-args -p n_iterations:=5 -p bag_path:=test_bag/ -p topics:="['/joint_states', '/d455_top/color/image_raw/image', '/d455_top/image_raw/camera_info']"
|
# Просто выполнить последовательность без записи
|
||||||
|
|
||||||
ros2 run iiwa_utils test_motion_sequence \
|
ros2 run iiwa_utils test_motion_sequence \
|
||||||
--ros-args -p config_path:=/path/to/my_config.json
|
--ros-args -p n_iterations:=3 \
|
||||||
|
-p delay_between_iterations:=5.0
|
||||||
|
|
||||||
|
# Записать все доступные топики в bag
|
||||||
|
ros2 run iiwa_utils test_motion_sequence \
|
||||||
|
--ros-args -p n_iterations:=5 \
|
||||||
|
-p delay_between_iterations:=5.0 \
|
||||||
|
-p bag_path:=/tmp/iiwa_session
|
||||||
|
|
||||||
|
# Записать конкретные топики
|
||||||
|
ros2 run iiwa_utils test_motion_sequence \
|
||||||
|
--ros-args -p n_iterations:=5 \
|
||||||
|
-p delay_between_iterations:=5.0 \
|
||||||
|
-p bag_path:=/tmp/iiwa_session \
|
||||||
|
-p topics:="['/joint_states', '/d455_top/color/image_raw', '/tf']"
|
||||||
|
|
||||||
|
# Использовать свой конфиг поз
|
||||||
|
ros2 run iiwa_utils test_motion_sequence \
|
||||||
|
--ros-args -p config_path:=/path/to/my_config.json \
|
||||||
|
-p n_iterations:=1 \
|
||||||
|
-p delay_between_iterations:=3.0 \
|
||||||
|
-p bag_path:=/tmp/iiwa_session
|
||||||
|
|
||||||
```
|
```
|
||||||
|
|
||||||
Спавн объекта:
|
Спавн объекта:
|
||||||
|
|||||||
@@ -36,6 +36,13 @@ def _setup_controllers(context, *args, **kwargs):
|
|||||||
if simulate:
|
if simulate:
|
||||||
tmo = ["--controller-manager-timeout", str(controller_timer)]
|
tmo = ["--controller-manager-timeout", str(controller_timer)]
|
||||||
|
|
||||||
|
spawner_urdf = URDFSpawner(
|
||||||
|
name=robot_name,
|
||||||
|
robot_description=robot_description,
|
||||||
|
translation=transform,
|
||||||
|
rotation=rotation,
|
||||||
|
)
|
||||||
|
|
||||||
jsb = Node(
|
jsb = Node(
|
||||||
package="controller_manager",
|
package="controller_manager",
|
||||||
executable="spawner",
|
executable="spawner",
|
||||||
@@ -60,14 +67,14 @@ def _setup_controllers(context, *args, **kwargs):
|
|||||||
parameters=[{"use_sim_time": True}]
|
parameters=[{"use_sim_time": True}]
|
||||||
)
|
)
|
||||||
|
|
||||||
spawner_urdf = URDFSpawner(
|
jtc_after_jsb = RegisterEventHandler(
|
||||||
name=robot_name,
|
OnProcessExit(
|
||||||
robot_description=robot_description,
|
target_action=jsb,
|
||||||
translation=transform,
|
on_exit=[jtc, torque_controller_spawner],
|
||||||
rotation=rotation,
|
)
|
||||||
)
|
)
|
||||||
|
|
||||||
return [jsb, jtc, torque_controller_spawner, spawner_urdf]
|
return [spawner_urdf, jsb, jtc_after_jsb]
|
||||||
|
|
||||||
# FRI
|
# FRI
|
||||||
else:
|
else:
|
||||||
@@ -113,10 +120,17 @@ def _setup_controllers(context, *args, **kwargs):
|
|||||||
arguments=torque_args,
|
arguments=torque_args,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
state_broadcaster = Node(
|
||||||
|
package="controller_manager",
|
||||||
|
executable="spawner",
|
||||||
|
output="screen",
|
||||||
|
arguments=["iiwa_state_broadcaster", "--controller-manager", "/controller_manager"],
|
||||||
|
)
|
||||||
|
|
||||||
jtc_after_jsb = RegisterEventHandler(
|
jtc_after_jsb = RegisterEventHandler(
|
||||||
OnProcessExit(
|
OnProcessExit(
|
||||||
target_action=jsb,
|
target_action=jsb,
|
||||||
on_exit=[jtc, torque_controller],
|
on_exit=[jtc, torque_controller, state_broadcaster],
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
@@ -34,7 +34,7 @@ controller:
|
|||||||
|
|
||||||
|
|
||||||
planning:
|
planning:
|
||||||
pose_link: "link_ee" # TCP-линк для декартовых целей
|
pose_link: "tcp" # TCP-линк для декартовых целей
|
||||||
planning_group: "iiwa_arm" # Группа планирования из SRDF
|
planning_group: "iiwa_arm" # Группа планирования из SRDF
|
||||||
default_frame: "base_link" # Система отсчёта по умолчанию
|
default_frame: "base_link" # Система отсчёта по умолчанию
|
||||||
default_planner: "ompl" # Планировщик по умолчанию
|
default_planner: "ompl" # Планировщик по умолчанию
|
||||||
|
|||||||
@@ -92,6 +92,7 @@
|
|||||||
</link>
|
</link>
|
||||||
|
|
||||||
<link name="camera_optical_frame"/>
|
<link name="camera_optical_frame"/>
|
||||||
|
<link name="tcp" />
|
||||||
|
|
||||||
<joint name="tool" type="fixed">
|
<joint name="tool" type="fixed">
|
||||||
<parent link="link_ee"/>
|
<parent link="link_ee"/>
|
||||||
@@ -105,6 +106,12 @@
|
|||||||
<child link="patron"/>
|
<child link="patron"/>
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
|
<joint name="patron_tcp" type="fixed">
|
||||||
|
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5708"/>
|
||||||
|
<parent link="patron"/>
|
||||||
|
<child link="tcp"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
<joint name="camera_holder_corner" type="fixed">
|
<joint name="camera_holder_corner" type="fixed">
|
||||||
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
|
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
|
||||||
<parent link="camera_holder"/>
|
<parent link="camera_holder"/>
|
||||||
|
|||||||
@@ -1,23 +1,19 @@
|
|||||||
package ros;
|
package ros;
|
||||||
|
|
||||||
// ════════════════════════════════════════════════════════════════════════════
|
// Sunrise OS 1.16 | FRI 1.16 | KUKA iiwa 7
|
||||||
//
|
//
|
||||||
// ServerFriRos2.java
|
// Режимы команды FRI:
|
||||||
// Sunrise OS 1.16 | FRI 1.16 | KUKA iiwa 7
|
// POSITION - ROS2 задаёт целевые углы суставов
|
||||||
|
// TORQUE - ROS2 задаёт добавочные моменты суставов
|
||||||
|
// NO_COMMAND_MODE - FRI только читает состояние, ведение рукой
|
||||||
//
|
//
|
||||||
// Режимы управления:
|
// Режимы управления Sunrise (только для POSITION и TORQUE):
|
||||||
// Position — FRI задаёт целевые углы суставов (PositionControlMode)
|
// POSITION_CONTROL - жёсткое позиционирование
|
||||||
// Сглаживание команд выполняется на стороне ROS2 (FRIClient EMA)
|
// JOINT_IMPEDANCE_CONTROL - упругое позиционирование с заданной жёсткостью
|
||||||
// Torque — FRI задаёт добавочные моменты (JointImpedanceControlMode)
|
|
||||||
// Monitor — FRI только читает состояние (NO_COMMAND_MODE)
|
|
||||||
// Масса инструмента берётся из Sunrise WB (Load Data)
|
|
||||||
// и валидируется через SmartServo.validateForImpedanceMode()
|
|
||||||
//
|
//
|
||||||
// Сетевые интерфейсы:
|
// Сетевые интерфейсы:
|
||||||
// KONI — 192.170.10.10 (рекомендуется)
|
// KONI - 192.170.10.10 (рекомендуется)
|
||||||
// KLI — 192.168.21.31
|
// KLI - 192.168.21.31
|
||||||
//
|
|
||||||
// ════════════════════════════════════════════════════════════════════════════
|
|
||||||
|
|
||||||
import java.util.concurrent.TimeUnit;
|
import java.util.concurrent.TimeUnit;
|
||||||
import java.util.concurrent.TimeoutException;
|
import java.util.concurrent.TimeoutException;
|
||||||
@@ -38,6 +34,7 @@ import com.kuka.roboticsAPI.deviceModel.LBR;
|
|||||||
import com.kuka.roboticsAPI.geometricModel.Tool;
|
import com.kuka.roboticsAPI.geometricModel.Tool;
|
||||||
import com.kuka.roboticsAPI.motionModel.BasicMotions;
|
import com.kuka.roboticsAPI.motionModel.BasicMotions;
|
||||||
import com.kuka.roboticsAPI.motionModel.PositionHold;
|
import com.kuka.roboticsAPI.motionModel.PositionHold;
|
||||||
|
import com.kuka.roboticsAPI.motionModel.controlModeModel.AbstractMotionControlMode;
|
||||||
import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode;
|
import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode;
|
||||||
import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode;
|
import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode;
|
||||||
import com.kuka.roboticsAPI.uiModel.ApplicationDialogType;
|
import com.kuka.roboticsAPI.uiModel.ApplicationDialogType;
|
||||||
@@ -45,88 +42,84 @@ import com.kuka.roboticsAPI.uiModel.ApplicationDialogType;
|
|||||||
|
|
||||||
public class ServerFriRos2 extends RoboticsAPIApplication {
|
public class ServerFriRos2 extends RoboticsAPIApplication {
|
||||||
|
|
||||||
|
// Позиции для движения перед запуском FRI
|
||||||
// ── КОНСТАНТЫ ────────────────────────────────────────────────────────────
|
private static final double[] ZERO_POSITION = {0, 0, 0, 0, 0, 0, 0};
|
||||||
|
|
||||||
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 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 KONI_IP = "192.170.10.10";
|
||||||
private static final String KLI_IP = "192.168.21.31";
|
private static final String KLI_IP = "192.168.21.31";
|
||||||
|
|
||||||
private static final int FRI_CONNECT_TIMEOUT_SEC = 30;
|
private static final int FRI_CONNECT_TIMEOUT_SEC = 30;
|
||||||
private static final double APPROACH_VEL = 0.30;
|
private static final double APPROACH_VEL = 0.30;
|
||||||
|
|
||||||
// Monitor режим: нулевая жёсткость = свободное ведение рукой
|
// Параметры для NO_COMMAND_MODE: нулевая жёсткость позволяет свободно вести робота рукой
|
||||||
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 {
|
||||||
|
POSITION,
|
||||||
|
TORQUE,
|
||||||
|
NO_COMMAND_MODE
|
||||||
|
}
|
||||||
|
|
||||||
|
// Режим управления Sunrise - как контроллер обрабатывает команды
|
||||||
|
private enum ControlMode {
|
||||||
|
POSITION_CONTROL,
|
||||||
|
JOINT_IMPEDANCE_CONTROL
|
||||||
|
}
|
||||||
|
|
||||||
|
// Сетевой интерфейс для подключения FRI
|
||||||
private enum NetworkInterface {
|
private enum NetworkInterface {
|
||||||
KONI("KONI (192.170.10.10) - выделенная FRI-сеть", KONI_IP),
|
KONI("KONI (192.170.10.10) - выделенная FRI-сеть", KONI_IP),
|
||||||
KLI ("KLI (192.168.21.31) - основная сеть KRC", KLI_IP);
|
KLI("KLI (192.168.21.31) - основная сеть KRC", KLI_IP);
|
||||||
|
|
||||||
final String label;
|
final String label;
|
||||||
final String ip;
|
final String ip;
|
||||||
|
|
||||||
NetworkInterface(String label, String ip) {
|
NetworkInterface(String label, String ip) {
|
||||||
this.label = label;
|
this.label = label;
|
||||||
this.ip = ip;
|
this.ip = ip;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private enum ControlMode {
|
|
||||||
POSITION,
|
|
||||||
TORQUE,
|
|
||||||
MONITOR
|
|
||||||
}
|
|
||||||
|
|
||||||
|
private LBR _lbr;
|
||||||
// ── ПОЛЯ ─────────────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
private LBR _lbr;
|
|
||||||
private Controller _lbrController;
|
private Controller _lbrController;
|
||||||
|
|
||||||
/**
|
// Инструмент из Sunrise WB - Object Templates tool1
|
||||||
* Инструмент, настроенный в Sunrise WB → Object Templates.
|
// Масса и CoM берутся из Load Data этого объекта
|
||||||
* Имя должно совпадать с именем в SWB (здесь: "patron").
|
|
||||||
* Масса и CoM берутся из Load Data этого объекта.
|
|
||||||
*/
|
|
||||||
@Inject
|
@Inject
|
||||||
@Named("patron")
|
@Named("tool1")
|
||||||
private Tool _tool;
|
private Tool _tool;
|
||||||
|
|
||||||
private NetworkInterface _selectedNetwork;
|
private NetworkInterface _selectedNetwork;
|
||||||
private ControlMode _selectedMode;
|
private CommandMode _selectedCommandMode;
|
||||||
private int _sendPeriodMs;
|
private ControlMode _selectedControlMode;
|
||||||
private double _jointStiffness;
|
private int _sendPeriodMs;
|
||||||
|
private double _jointStiffness;
|
||||||
|
|
||||||
private FRIConfiguration _friConfig;
|
private FRIConfiguration _friConfig;
|
||||||
private FRISession _friSession;
|
private FRISession _friSession;
|
||||||
private FRIJointOverlay _friOverlay;
|
private FRIJointOverlay _friOverlay;
|
||||||
private IFRISessionListener _friListener;
|
private IFRISessionListener _friListener;
|
||||||
|
|
||||||
|
|
||||||
// ── LIFECYCLE ────────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void initialize() {
|
public void initialize() {
|
||||||
|
|
||||||
_lbrController = (Controller) getContext().getControllers().toArray()[0];
|
_lbrController = (Controller) getContext().getControllers().toArray()[0];
|
||||||
_lbr = (LBR) _lbrController.getDevices().toArray()[0];
|
_lbr = (LBR) _lbrController.getDevices().toArray()[0];
|
||||||
|
|
||||||
// Прикрепляем инструмент к фланцу — обязательно для корректной
|
// Прикрепляем инструмент к фланцу для корректной гравкомпенсации
|
||||||
// гравкомпенсации и validateForImpedanceMode()
|
|
||||||
_tool.attachTo(_lbr.getFlange());
|
_tool.attachTo(_lbr.getFlange());
|
||||||
|
|
||||||
getLogger().info("════════════════════════════════════════════");
|
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());
|
|
||||||
getLogger().info("════════════════════════════════════════════");
|
|
||||||
|
|
||||||
initFriListener();
|
initFriListener();
|
||||||
requestUserConfig();
|
requestUserConfig();
|
||||||
@@ -137,10 +130,10 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
|
|||||||
|
|
||||||
moveToInitialPosition();
|
moveToInitialPosition();
|
||||||
|
|
||||||
switch (_selectedMode) {
|
switch (_selectedCommandMode) {
|
||||||
case POSITION: runPositionMode(); break;
|
case POSITION: runPositionMode(); break;
|
||||||
case TORQUE: runTorqueMode(); break;
|
case TORQUE: runTorqueMode(); break;
|
||||||
case MONITOR: runMonitorMode(); break;
|
case NO_COMMAND_MODE: runMonitorMode(); break;
|
||||||
}
|
}
|
||||||
|
|
||||||
getLogger().info("Программа завершена.");
|
getLogger().info("Программа завершена.");
|
||||||
@@ -150,7 +143,7 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
|
|||||||
public void dispose() {
|
public void dispose() {
|
||||||
|
|
||||||
if (_friSession != null) {
|
if (_friSession != null) {
|
||||||
getLogger().info("dispose(): Закрытие FRI-сессии...");
|
getLogger().info("Закрытие FRI-сессии...");
|
||||||
try {
|
try {
|
||||||
_friSession.close();
|
_friSession.close();
|
||||||
} catch (Exception e) {
|
} catch (Exception e) {
|
||||||
@@ -163,107 +156,152 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// ── UI — ЗАПРОС КОНФИГУРАЦИИ ─────────────────────────────────────────────
|
// Последовательный опрос конфигурации - каждый шаг зависит от предыдущего
|
||||||
|
|
||||||
private void requestUserConfig() {
|
private void requestUserConfig() {
|
||||||
|
|
||||||
// Шаг 1: Сетевой интерфейс
|
// Шаг 1: сетевой интерфейс
|
||||||
int netChoice = getApplicationUI().displayModalDialog(
|
int netChoice = getApplicationUI().displayModalDialog(
|
||||||
ApplicationDialogType.QUESTION,
|
ApplicationDialogType.QUESTION,
|
||||||
"Шаг 1 / 3 — Сетевой интерфейс FRI\n\n"
|
"Шаг 1 - Сетевой интерфейс FRI\n\n"
|
||||||
+ "KONI: выделенная высокоскоростная сеть (рекомендуется)\n"
|
+ "KONI: выделенная высокоскоростная сеть (рекомендуется)\n"
|
||||||
+ "KLI : основная сеть KRC",
|
+ "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("[Шаг 1] Сеть: " + _selectedNetwork.label);
|
getLogger().info("Сетевой интерфейс: " + _selectedNetwork.label);
|
||||||
|
|
||||||
// Шаг 2: Режим управления
|
// Шаг 2: режим команды FRI
|
||||||
int modeChoice = getApplicationUI().displayModalDialog(
|
int modeChoice = getApplicationUI().displayModalDialog(
|
||||||
ApplicationDialogType.QUESTION,
|
ApplicationDialogType.QUESTION,
|
||||||
"Шаг 2 / 3 — Режим управления FRI\n\n"
|
"Шаг 2 - Режим команды FRI\n\n"
|
||||||
+ "Position : ROS2 задаёт угловые позиции суставов\n"
|
+ "Position: ROS2 задаёт угловые позиции суставов\n"
|
||||||
+ "Torque : ROS2 задаёт добавочные моменты\n"
|
+ "Torque: ROS2 задаёт добавочные моменты суставов\n"
|
||||||
+ "Monitor : только данные; ведение рукой",
|
+ "Monitor: только чтение, ведение рукой (NO_COMMAND_MODE)",
|
||||||
"Position",
|
"Position",
|
||||||
"Torque",
|
"Torque",
|
||||||
"Monitor"
|
"Monitor"
|
||||||
);
|
);
|
||||||
|
|
||||||
if (modeChoice == 0) {
|
if (modeChoice == 0) {
|
||||||
_selectedMode = ControlMode.POSITION;
|
_selectedCommandMode = CommandMode.POSITION;
|
||||||
_jointStiffness = 0;
|
selectControlMode();
|
||||||
|
selectSendPeriodForPosition();
|
||||||
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) {
|
} else if (modeChoice == 1) {
|
||||||
_selectedMode = ControlMode.TORQUE;
|
_selectedCommandMode = CommandMode.TORQUE;
|
||||||
|
// TORQUE всегда требует JointImpedanceControlMode на стороне Sunrise
|
||||||
int pChoice = getApplicationUI().displayModalDialog(
|
_selectedControlMode = ControlMode.JOINT_IMPEDANCE_CONTROL;
|
||||||
ApplicationDialogType.QUESTION,
|
selectJointStiffness();
|
||||||
"Шаг 3а / 4 — Период отправки [мс] (Torque)\n\n"
|
selectSendPeriodForTorque();
|
||||||
+ "При пропуске пакета 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 {
|
} else {
|
||||||
_selectedMode = ControlMode.MONITOR;
|
_selectedCommandMode = CommandMode.NO_COMMAND_MODE;
|
||||||
_sendPeriodMs = 2; // 2 мс — запас для non-RT систем без FIFO-планировщика
|
// Нулевая жёсткость задана константой, пользователю выбирать нечего
|
||||||
_jointStiffness = 0;
|
_selectedControlMode = ControlMode.JOINT_IMPEDANCE_CONTROL;
|
||||||
getLogger().info("[Шаг 3] Monitor: период = " + _sendPeriodMs + " мс");
|
_sendPeriodMs = 2;
|
||||||
|
_jointStiffness = 0.0;
|
||||||
|
getLogger().info("Monitor (NO_COMMAND_MODE): период = " + _sendPeriodMs + " мс");
|
||||||
}
|
}
|
||||||
|
|
||||||
getLogger().info("════════════════════════════════════════════");
|
logConfiguration();
|
||||||
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("════════════════════════════════════════════");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// ── ДВИЖЕНИЕ В СТАРТОВУЮ ПОЗИЦИЮ ─────────────────────────────────────────
|
// Шаг 3a (только для POSITION): выбор режима управления Sunrise
|
||||||
|
private void selectControlMode() {
|
||||||
|
|
||||||
|
int ctrlChoice = getApplicationUI().displayModalDialog(
|
||||||
|
ApplicationDialogType.QUESTION,
|
||||||
|
"Шаг 3 - Режим управления Sunrise (Position)\n\n"
|
||||||
|
+ "PositionControl: жёсткое позиционирование, максимальная точность следования\n"
|
||||||
|
+ "JointImpedance: упругое позиционирование, задаётся жёсткость суставов",
|
||||||
|
"PositionControl",
|
||||||
|
"JointImpedance"
|
||||||
|
);
|
||||||
|
|
||||||
|
if (ctrlChoice == 0) {
|
||||||
|
_selectedControlMode = ControlMode.POSITION_CONTROL;
|
||||||
|
_jointStiffness = 0.0;
|
||||||
|
getLogger().info("Режим управления: PositionControlMode");
|
||||||
|
} else {
|
||||||
|
_selectedControlMode = ControlMode.JOINT_IMPEDANCE_CONTROL;
|
||||||
|
selectJointStiffness();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Выбор жёсткости суставов для JointImpedanceControlMode
|
||||||
|
private void selectJointStiffness() {
|
||||||
|
|
||||||
|
int sChoice = getApplicationUI().displayModalDialog(
|
||||||
|
ApplicationDialogType.QUESTION,
|
||||||
|
"Жёсткость суставов [Нм/рад]\n\n"
|
||||||
|
+ "Высокая жёсткость: точное следование, меньше отклонение от траектории\n"
|
||||||
|
+ "Низкая жёсткость: мягкое взаимодействие со средой\n\n"
|
||||||
|
+ "Внимание: 1500 Нм/рад только в производственном режиме без людей в зоне!",
|
||||||
|
"1500 - жёсткий / производственный",
|
||||||
|
"1000 - стандарт",
|
||||||
|
"800 - средний",
|
||||||
|
"500 - мягкий / взаимодействие",
|
||||||
|
"300 - очень мягкий"
|
||||||
|
);
|
||||||
|
_jointStiffness = new double[]{1500, 1000, 800, 500, 300}[sChoice];
|
||||||
|
getLogger().info("Жёсткость суставов: " + _jointStiffness + " Нм/рад");
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
private void selectSendPeriodForPosition() {
|
||||||
|
|
||||||
|
int pChoice = getApplicationUI().displayModalDialog(
|
||||||
|
ApplicationDialogType.QUESTION,
|
||||||
|
"Шаг 4 - Период отправки FRI [мс] (Position)\n\n"
|
||||||
|
+ "10 мс: стабильно, подходит для большинства сетей\n"
|
||||||
|
+ "5 мс: стандарт для ros2_control на 200 Гц\n"
|
||||||
|
+ "2 мс: быстро, требует низкого джиттера сети",
|
||||||
|
"10 мс",
|
||||||
|
"5 мс",
|
||||||
|
"2 мс"
|
||||||
|
);
|
||||||
|
_sendPeriodMs = new int[]{10, 5, 2}[pChoice];
|
||||||
|
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 + " мс");
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
private void logConfiguration() {
|
||||||
|
|
||||||
|
getLogger().info("Итоговая конфигурация:");
|
||||||
|
getLogger().info(" Сеть: " + _selectedNetwork.label);
|
||||||
|
getLogger().info(" IP: " + _selectedNetwork.ip);
|
||||||
|
getLogger().info(" Режим команды FRI: " + _selectedCommandMode.name());
|
||||||
|
getLogger().info(" Режим управления Sunrise: " + _selectedControlMode.name());
|
||||||
|
getLogger().info(" Период отправки: " + _sendPeriodMs + " мс");
|
||||||
|
getLogger().info(" Инструмент: " + _tool.getName());
|
||||||
|
if (_selectedControlMode == ControlMode.JOINT_IMPEDANCE_CONTROL && _jointStiffness > 0) {
|
||||||
|
getLogger().info(" Жёсткость суставов: " + _jointStiffness + " Нм/рад");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
private void moveToInitialPosition() {
|
private void moveToInitialPosition() {
|
||||||
|
|
||||||
if (_selectedMode == ControlMode.MONITOR) {
|
if (_selectedCommandMode == CommandMode.NO_COMMAND_MODE) {
|
||||||
|
|
||||||
getLogger().info("Движение в рабочую позицию Monitor (через нулевую)...");
|
getLogger().info("Движение в рабочую позицию Monitor (через нулевую)...");
|
||||||
_lbr.move(
|
_lbr.move(
|
||||||
BasicMotions.batch(
|
BasicMotions.batch(
|
||||||
@@ -272,9 +310,7 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
|
|||||||
).setJointVelocityRel(APPROACH_VEL)
|
).setJointVelocityRel(APPROACH_VEL)
|
||||||
);
|
);
|
||||||
getLogger().info("Рабочая позиция Monitor достигнута.");
|
getLogger().info("Рабочая позиция Monitor достигнута.");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
getLogger().info("Движение в нулевую позицию...");
|
getLogger().info("Движение в нулевую позицию...");
|
||||||
_lbr.move(
|
_lbr.move(
|
||||||
BasicMotions.ptp(ZERO_POSITION).setJointVelocityRel(APPROACH_VEL)
|
BasicMotions.ptp(ZERO_POSITION).setJointVelocityRel(APPROACH_VEL)
|
||||||
@@ -284,20 +320,18 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// ── РЕЖИМ: POSITION ──────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
private void runPositionMode() {
|
private void runPositionMode() {
|
||||||
getLogger().info("═══ Position режим (PositionControlMode) ═══");
|
|
||||||
|
getLogger().info("Запуск Position режима, управление: " + _selectedControlMode.name());
|
||||||
|
|
||||||
if (!setupFriSession(ClientCommandMode.POSITION)) {
|
if (!setupFriSession(ClientCommandMode.POSITION)) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
PositionControlMode ctrlMode = new PositionControlMode();
|
AbstractMotionControlMode ctrlMode = buildControlMode();
|
||||||
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));
|
_lbr.move(posHold.addMotionOverlay(_friOverlay));
|
||||||
|
|
||||||
_friSession.close();
|
_friSession.close();
|
||||||
@@ -306,11 +340,9 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// ── РЕЖИМ: TORQUE ────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
private void runTorqueMode() {
|
private void runTorqueMode() {
|
||||||
getLogger().info("═══ Torque режим ═══");
|
|
||||||
getLogger().info("Жёсткость: " + _jointStiffness + " Нм/рад");
|
getLogger().info("Запуск Torque режима, жёсткость: " + _jointStiffness + " Нм/рад");
|
||||||
|
|
||||||
if (!setupFriSession(ClientCommandMode.TORQUE)) {
|
if (!setupFriSession(ClientCommandMode.TORQUE)) {
|
||||||
return;
|
return;
|
||||||
@@ -326,7 +358,6 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
|
|||||||
PositionHold posHold = new PositionHold(ctrlMode, -1, TimeUnit.SECONDS);
|
PositionHold posHold = new PositionHold(ctrlMode, -1, TimeUnit.SECONDS);
|
||||||
|
|
||||||
getLogger().info("Torque режим активен. Ожидаю команды от ROS2...");
|
getLogger().info("Torque режим активен. Ожидаю команды от ROS2...");
|
||||||
|
|
||||||
_lbr.move(posHold.addMotionOverlay(_friOverlay));
|
_lbr.move(posHold.addMotionOverlay(_friOverlay));
|
||||||
|
|
||||||
_friSession.close();
|
_friSession.close();
|
||||||
@@ -335,41 +366,27 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// ── РЕЖИМ: 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() {
|
private void runMonitorMode() {
|
||||||
getLogger().info("═══ Monitor режим ═══");
|
|
||||||
|
|
||||||
// Фаза A: валидация Load Data инструмента из SWB
|
getLogger().info("Запуск Monitor режима (NO_COMMAND_MODE).");
|
||||||
|
|
||||||
|
// Фаза A: валидация Load Data инструмента из Sunrise WB
|
||||||
validateLoadModel();
|
validateLoadModel();
|
||||||
|
|
||||||
// Фаза B: ждём подтверждения оператора что ROS2 FRI готов
|
// Фаза 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 в режиме только чтения
|
// Фаза C: FRI в режиме только чтения (NO_COMMAND_MODE)
|
||||||
if (!setupFriSession(ClientCommandMode.NO_COMMAND_MODE)) {
|
if (!setupFriSession(ClientCommandMode.NO_COMMAND_MODE)) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Фаза D: PositionHold с нулевой жёсткостью — свободное ведение рукой
|
// Фаза D: PositionHold с нулевой жёсткостью - свободное ведение рукой
|
||||||
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,
|
||||||
@@ -379,11 +396,10 @@ 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("Ведите робота рукой - он не сопротивляется.");
|
||||||
getLogger().info(" • Данные суставов транслируются в ROS2 каждые "
|
getLogger().info("Данные суставов транслируются в ROS2 каждые " + _sendPeriodMs + " мс.");
|
||||||
+ _sendPeriodMs + " мс.");
|
getLogger().info("Остановите FRI-клиент для завершения.");
|
||||||
getLogger().info(" • Остановите FRI-клиент для завершения.");
|
|
||||||
|
|
||||||
_lbr.move(posHold);
|
_lbr.move(posHold);
|
||||||
|
|
||||||
@@ -393,47 +409,50 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// ── ВАЛИДАЦИЯ НАГРУЗКИ ИНСТРУМЕНТА ──────────────────────────────────────
|
// Создаёт объект режима управления на основе выбора пользователя
|
||||||
|
private AbstractMotionControlMode buildControlMode() {
|
||||||
|
|
||||||
/**
|
if (_selectedControlMode == ControlMode.POSITION_CONTROL) {
|
||||||
* Проверяет Load Data (масса / CoM / инерция) инструмента из Sunrise WB.
|
getLogger().info("Создан PositionControlMode.");
|
||||||
*
|
return new PositionControlMode();
|
||||||
* Аналог validateLoadModel() из TeachKuka.java:
|
}
|
||||||
* SmartServo.validateForImpedanceMode(_tool) проверяет, что данные
|
|
||||||
* нагрузки заданы корректно для работы с JointImpedanceControlMode.
|
// JointImpedanceControlMode - одинаковая жёсткость для всех суставов
|
||||||
*
|
JointImpedanceControlMode mode = new JointImpedanceControlMode(
|
||||||
* Если валидация не прошла — нужно задать Load Data в:
|
_jointStiffness, _jointStiffness, _jointStiffness,
|
||||||
* Sunrise WB → Object Templates → patron → Load Data
|
_jointStiffness, _jointStiffness, _jointStiffness,
|
||||||
* (Mass, Centre of Mass, Moment of Inertia)
|
_jointStiffness
|
||||||
*/
|
);
|
||||||
|
mode.setDampingForAllJoints(0.7);
|
||||||
|
getLogger().info("Создан JointImpedanceControlMode, жёсткость = " + _jointStiffness + " Нм/рад");
|
||||||
|
return mode;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Проверяет Load Data инструмента из Sunrise WB через SmartServo.validateForImpedanceMode.
|
||||||
|
// Корректные данные нагрузки обязательны для точной гравкомпенсации в Monitor режиме.
|
||||||
private void validateLoadModel() {
|
private void validateLoadModel() {
|
||||||
getLogger().info("════════════════════════════════════════════");
|
|
||||||
getLogger().info(" ВАЛИДАЦИЯ НАГРУЗКИ ИНСТРУМЕНТА");
|
getLogger().info("Валидация нагрузки инструмента: " + _tool.getName());
|
||||||
getLogger().info(" Инструмент: " + _tool.getName());
|
|
||||||
getLogger().info("════════════════════════════════════════════");
|
|
||||||
|
|
||||||
boolean valid = SmartServo.validateForImpedanceMode(_tool);
|
boolean valid = SmartServo.validateForImpedanceMode(_tool);
|
||||||
|
|
||||||
if (valid) {
|
if (valid) {
|
||||||
getLogger().info(" ✓ Load Data валидны.");
|
getLogger().info("Load Data валидны. Гравкомпенсация будет работать точно.");
|
||||||
getLogger().info(" Масса и CoM заданы корректно в Sunrise WB.");
|
|
||||||
getLogger().info(" Гравкомпенсация будет работать точно.");
|
|
||||||
} else {
|
} else {
|
||||||
getLogger().warn(" ⚠ Валидация Load Data НЕ прошла!");
|
getLogger().warn("Валидация Load Data не прошла для инструмента: " + _tool.getName());
|
||||||
getLogger().warn(" Задайте данные нагрузки в:");
|
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 [кг/м2])");
|
||||||
getLogger().warn(" (Mass [кг], Centre of Mass [мм], Inertia [кг·м²])");
|
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затем перезапустите программу.\n\n"
|
+ _tool.getName() + " -> Load Data\nи перезапустите программу.\n\n"
|
||||||
+ "Или продолжите без корректной нагрузки (на свой риск).",
|
+ "Или продолжите без корректной нагрузки (на свой риск).",
|
||||||
"Продолжить",
|
"Продолжить",
|
||||||
"Остановить"
|
"Остановить"
|
||||||
@@ -441,17 +460,12 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
|
|||||||
|
|
||||||
if (choice == 1) {
|
if (choice == 1) {
|
||||||
throw new RuntimeException(
|
throw new RuntimeException(
|
||||||
"Остановлено оператором: Load Data не заданы для "
|
"Остановлено оператором: Load Data не заданы для " + _tool.getName());
|
||||||
+ _tool.getName());
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
getLogger().info("════════════════════════════════════════════");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// ── FRI — НАСТРОЙКА СЕССИИ ───────────────────────────────────────────────
|
|
||||||
|
|
||||||
private boolean setupFriSession(ClientCommandMode commandMode) {
|
private boolean setupFriSession(ClientCommandMode commandMode) {
|
||||||
|
|
||||||
_friConfig = FRIConfiguration.createRemoteConfiguration(_lbr, _selectedNetwork.ip);
|
_friConfig = FRIConfiguration.createRemoteConfiguration(_lbr, _selectedNetwork.ip);
|
||||||
@@ -459,10 +473,10 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
|
|||||||
_friConfig.setReceiveMultiplier(1);
|
_friConfig.setReceiveMultiplier(1);
|
||||||
|
|
||||||
getLogger().info("Создание FRI-сессии...");
|
getLogger().info("Создание FRI-сессии...");
|
||||||
getLogger().info(" Хост : " + _friConfig.getHostName()
|
getLogger().info("Хост: " + _friConfig.getHostName()
|
||||||
+ " порт: " + _friConfig.getPortOnRemote());
|
+ ", порт: " + _friConfig.getPortOnRemote());
|
||||||
getLogger().info(" Режим : " + commandMode.name());
|
getLogger().info("Режим команды: " + commandMode.name());
|
||||||
getLogger().info(" Период : " + _friConfig.getSendPeriodMilliSec() + " мс");
|
getLogger().info("Период отправки: " + _friConfig.getSendPeriodMilliSec() + " мс");
|
||||||
|
|
||||||
_friSession = new FRISession(_friConfig);
|
_friSession = new FRISession(_friConfig);
|
||||||
_friSession.addFRISessionListener(_friListener);
|
_friSession.addFRISessionListener(_friListener);
|
||||||
@@ -471,12 +485,9 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
|
|||||||
getLogger().info("Ожидание FRI-клиента на " + _selectedNetwork.ip
|
getLogger().info("Ожидание FRI-клиента на " + _selectedNetwork.ip
|
||||||
+ " (таймаут " + FRI_CONNECT_TIMEOUT_SEC + " с)...");
|
+ " (таймаут " + FRI_CONNECT_TIMEOUT_SEC + " с)...");
|
||||||
_friSession.await(FRI_CONNECT_TIMEOUT_SEC, TimeUnit.SECONDS);
|
_friSession.await(FRI_CONNECT_TIMEOUT_SEC, TimeUnit.SECONDS);
|
||||||
|
|
||||||
} catch (TimeoutException e) {
|
} catch (TimeoutException e) {
|
||||||
getLogger().error("Таймаут FRI! Клиент не ответил за "
|
getLogger().error("Таймаут FRI! Клиент не ответил за " + FRI_CONNECT_TIMEOUT_SEC + " с.");
|
||||||
+ FRI_CONNECT_TIMEOUT_SEC + " с.");
|
getLogger().error("Убедитесь, что ROS2 FRI-узел запущен на " + _selectedNetwork.ip);
|
||||||
getLogger().error("Убедитесь, что ROS2 FRI-узел запущен на "
|
|
||||||
+ _selectedNetwork.ip);
|
|
||||||
_friSession.close();
|
_friSession.close();
|
||||||
_friSession = null;
|
_friSession = null;
|
||||||
return false;
|
return false;
|
||||||
@@ -487,43 +498,46 @@ public class ServerFriRos2 extends RoboticsAPIApplication {
|
|||||||
|
|
||||||
if (commandMode != ClientCommandMode.NO_COMMAND_MODE) {
|
if (commandMode != ClientCommandMode.NO_COMMAND_MODE) {
|
||||||
_friOverlay = new FRIJointOverlay(_friSession, commandMode);
|
_friOverlay = new FRIJointOverlay(_friSession, commandMode);
|
||||||
getLogger().info("FRIJointOverlay создан: " + commandMode.name());
|
getLogger().info("FRIJointOverlay создан для режима: " + commandMode.name());
|
||||||
} else {
|
} else {
|
||||||
_friOverlay = null;
|
_friOverlay = null;
|
||||||
getLogger().info("Monitor: FRIJointOverlay не создаётся (только чтение).");
|
getLogger().info("Monitor: FRIJointOverlay не создаётся (только чтение данных).");
|
||||||
}
|
}
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// ── FRI — LISTENER ───────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
private void initFriListener() {
|
private void initFriListener() {
|
||||||
_friListener = new IFRISessionListener() {
|
_friListener = new IFRISessionListener() {
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void onFRIConnectionQualityChanged(FRIChannelInformation info) {
|
public void onFRIConnectionQualityChanged(FRIChannelInformation info) {
|
||||||
getLogger().info("[FRI] Качество: " + info.getQuality()
|
getLogger().info("FRI качество изменилось: " + 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 состояние изменилось: " + 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 состояние: " + info.getFRISessionState());
|
||||||
getLogger().info("[FRI] Качество : " + info.getQuality());
|
getLogger().info("FRI качество: " + 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() + " мс");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
public static void main(final String[] args) {
|
||||||
|
ServerFriRos2 app = new ServerFriRos2();
|
||||||
|
app.runApplication();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,185 +0,0 @@
|
|||||||
// Copyright 2022, ICube Laboratory, University of Strasbourg
|
|
||||||
//
|
|
||||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
||||||
// you may not use this file except in compliance with the License.
|
|
||||||
// You may obtain a copy of the License at
|
|
||||||
//
|
|
||||||
// http://www.apache.org/licenses/LICENSE-2.0
|
|
||||||
//
|
|
||||||
// Unless required by applicable law or agreed to in writing, software
|
|
||||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
||||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
||||||
// See the License for the specific language governing permissions and
|
|
||||||
// limitations under the License.
|
|
||||||
|
|
||||||
package application;
|
|
||||||
|
|
||||||
import java.util.concurrent.TimeUnit;
|
|
||||||
import java.util.concurrent.TimeoutException;
|
|
||||||
|
|
||||||
import javax.inject.Inject;
|
|
||||||
import javax.inject.Named;
|
|
||||||
|
|
||||||
import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
|
|
||||||
import static com.kuka.roboticsAPI.motionModel.BasicMotions.*;
|
|
||||||
|
|
||||||
import com.kuka.roboticsAPI.conditionModel.BooleanIOCondition;
|
|
||||||
import com.kuka.roboticsAPI.controllerModel.Controller;
|
|
||||||
import com.kuka.roboticsAPI.deviceModel.JointPosition;
|
|
||||||
import com.kuka.roboticsAPI.deviceModel.LBR;
|
|
||||||
import com.kuka.roboticsAPI.geometricModel.Tool;
|
|
||||||
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;
|
|
||||||
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.generated.flexfellow.FlexFellow;
|
|
||||||
import com.kuka.generated.ioAccess.MediaFlangeIOGroup;
|
|
||||||
import com.kuka.grippertoolbox.api.gripper.AbstractGripper;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Implementation of a robot application.
|
|
||||||
* <p>
|
|
||||||
* The application provides a {@link RoboticsAPITask#initialize()} and a
|
|
||||||
* {@link RoboticsAPITask#run()} method, which will be called successively in
|
|
||||||
* the application lifecycle. The application will terminate automatically after
|
|
||||||
* the {@link RoboticsAPITask#run()} method has finished or after stopping the
|
|
||||||
* task. The {@link RoboticsAPITask#dispose()} method will be called, even if an
|
|
||||||
* exception is thrown during initialization or run.
|
|
||||||
* <p>
|
|
||||||
* <b>It is imperative to call <code>super.dispose()</code> when overriding the
|
|
||||||
* {@link RoboticsAPITask#dispose()} method.</b>
|
|
||||||
*
|
|
||||||
* @see UseRoboticsAPIContext
|
|
||||||
* @see #initialize()
|
|
||||||
* @see #run()
|
|
||||||
* @see #dispose()
|
|
||||||
*/
|
|
||||||
public class Iiwa_ros2 extends RoboticsAPIApplication {
|
|
||||||
private Controller _lbrController;
|
|
||||||
private LBR _lbr;
|
|
||||||
private String _clientName;
|
|
||||||
@Inject
|
|
||||||
private MediaFlangeIOGroup _medflange;
|
|
||||||
|
|
||||||
PositionHold posHold;
|
|
||||||
FRIJointOverlay jointOverlay;
|
|
||||||
|
|
||||||
private static final JointPosition INITIAL_POSITION = new JointPosition(0.0,-0.7854,0.0,1.3962,0.0,0.6109,0.0);
|
|
||||||
private static final String CLIENT_IP = "192.170.10.5";
|
|
||||||
private static final double TS = 5; //in ms
|
|
||||||
|
|
||||||
IFRISessionListener listener = new IFRISessionListener(){
|
|
||||||
@Override
|
|
||||||
public void onFRIConnectionQualityChanged(
|
|
||||||
FRIChannelInformation friChannelInformation){
|
|
||||||
getLogger().info("QualityChangedEvent - quality:" +
|
|
||||||
friChannelInformation.getQuality()+"\n Jitter info:" + friChannelInformation.getJitter() +"\n Latency info:" + friChannelInformation.getLatency());
|
|
||||||
}
|
|
||||||
@Override
|
|
||||||
public void onFRISessionStateChanged(
|
|
||||||
FRIChannelInformation friChannelInformation){
|
|
||||||
getLogger().info("SessionStateChangedEvent - session state:" +
|
|
||||||
friChannelInformation.getFRISessionState() +"\n Jitter info:" + friChannelInformation.getJitter() +"\n Latency info:" + friChannelInformation.getLatency());
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void initialize() {
|
|
||||||
_lbrController = (Controller) getContext().getControllers().toArray()[0];
|
|
||||||
_lbr = (LBR) _lbrController.getDevices().toArray()[0];
|
|
||||||
// **********************************************************************
|
|
||||||
// *** change next line to the FRIClient's IP address ***
|
|
||||||
// **********************************************************************
|
|
||||||
_clientName = CLIENT_IP;
|
|
||||||
_lbr.attachTo(_lbr.getFlange());
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void run() {
|
|
||||||
// Select the type of control
|
|
||||||
String ques = "Select FRI control mode :\n";
|
|
||||||
double res = getApplicationUI().displayModalDialog(ApplicationDialogType.QUESTION,ques , "POSITION","TORQUE","MONITORING","Cancel");
|
|
||||||
|
|
||||||
_medflange.setLEDRed(true);
|
|
||||||
|
|
||||||
_lbr.move(ptp(INITIAL_POSITION).setJointVelocityRel(0.2));
|
|
||||||
|
|
||||||
if(res == 0){
|
|
||||||
PositionControlMode ctrMode = new PositionControlMode();
|
|
||||||
posHold = new PositionHold(ctrMode, -1, TimeUnit.MINUTES);
|
|
||||||
}
|
|
||||||
else if (res == 1 || res == 2){
|
|
||||||
JointImpedanceControlMode ctrMode = new JointImpedanceControlMode(0.0,0.0,0.0,0.0,0.0,0.0,0.0);
|
|
||||||
ctrMode.setStiffnessForAllJoints(0.0);
|
|
||||||
posHold = new PositionHold(ctrMode, -1, TimeUnit.MINUTES);
|
|
||||||
}
|
|
||||||
else return;
|
|
||||||
|
|
||||||
// configure and start FRI session
|
|
||||||
FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(_lbr, _clientName);
|
|
||||||
// for torque mode, there has to be a command value at least all 5ms
|
|
||||||
friConfiguration.setSendPeriodMilliSec(TS);
|
|
||||||
friConfiguration.setReceiveMultiplier(1);
|
|
||||||
|
|
||||||
getLogger().info("Creating FRI connection to " + friConfiguration.getHostName());
|
|
||||||
getLogger().info("SendPeriod: " + friConfiguration.getSendPeriodMilliSec() + "ms |"
|
|
||||||
+ " ReceiveMultiplier: " + friConfiguration.getReceiveMultiplier());
|
|
||||||
|
|
||||||
FRISession friSession = new FRISession(friConfiguration);
|
|
||||||
friSession.addFRISessionListener(listener);
|
|
||||||
|
|
||||||
// wait until FRI session is ready to switch to command mode
|
|
||||||
try
|
|
||||||
{
|
|
||||||
friSession.await(20, TimeUnit.SECONDS);
|
|
||||||
}
|
|
||||||
catch (final TimeoutException e)
|
|
||||||
{
|
|
||||||
getLogger().error(e.getLocalizedMessage());
|
|
||||||
friSession.close();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
getLogger().info("FRI connection established.");
|
|
||||||
|
|
||||||
getLogger().info("Jitter info: " + friSession.getFRIChannelInformation().getJitter());
|
|
||||||
|
|
||||||
if(res == 0){
|
|
||||||
jointOverlay = new FRIJointOverlay(friSession, ClientCommandMode.POSITION);
|
|
||||||
}
|
|
||||||
else if (res == 1){
|
|
||||||
jointOverlay = new FRIJointOverlay(friSession, ClientCommandMode.TORQUE);
|
|
||||||
}
|
|
||||||
else if(res== 2){
|
|
||||||
jointOverlay = new FRIJointOverlay(friSession, ClientCommandMode.NO_COMMAND_MODE);
|
|
||||||
}
|
|
||||||
else return;
|
|
||||||
|
|
||||||
|
|
||||||
_medflange.setLEDRed(false);
|
|
||||||
_medflange.setLEDGreen(true);
|
|
||||||
BooleanIOCondition _buttonPressed = new BooleanIOCondition(_medflange.getInput("UserButton"), true);
|
|
||||||
if(res == 0 || res == 1) _lbr.move(posHold.addMotionOverlay(jointOverlay).breakWhen(_buttonPressed));
|
|
||||||
else _lbr.move(posHold.breakWhen(_buttonPressed));
|
|
||||||
|
|
||||||
_medflange.setLEDGreen(false);
|
|
||||||
_medflange.setLEDRed(true);
|
|
||||||
// done
|
|
||||||
friSession.close();
|
|
||||||
getLogger().info("FRI connection closed.");
|
|
||||||
getLogger().info("Application stopped.");
|
|
||||||
}
|
|
||||||
|
|
||||||
public static void main(final String[] args)
|
|
||||||
{
|
|
||||||
final Iiwa_ros2 app = new Iiwa_ros2();
|
|
||||||
app.runApplication();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,5 +1,6 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
import json
|
import json
|
||||||
|
import shutil
|
||||||
import threading
|
import threading
|
||||||
import time
|
import time
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
@@ -22,20 +23,17 @@ class IiwaTestRunner(Node):
|
|||||||
super().__init__('iiwa_test_runner')
|
super().__init__('iiwa_test_runner')
|
||||||
|
|
||||||
self.declare_parameter('n_iterations', 3)
|
self.declare_parameter('n_iterations', 3)
|
||||||
self.declare_parameter('bag_path', '/tmp/iiwa_test')
|
self.declare_parameter('bag_path', '')
|
||||||
self.declare_parameter('config_path', '')
|
self.declare_parameter('config_path', '')
|
||||||
self.declare_parameter('topics', [
|
self.declare_parameter('topics', [''])
|
||||||
'/joint_states',
|
|
||||||
'/iiwa/joint_states',
|
|
||||||
'/tf',
|
|
||||||
'/tf_static',
|
|
||||||
])
|
|
||||||
self.declare_parameter("delay_between_iterations", 5.0)
|
self.declare_parameter("delay_between_iterations", 5.0)
|
||||||
|
|
||||||
self._n_iter = self.get_parameter('n_iterations').value
|
self._n_iter = self.get_parameter('n_iterations').value
|
||||||
self._delay_between_iterations = self.get_parameter("delay_between_iterations").value
|
self._delay_between_iterations = self.get_parameter("delay_between_iterations").value
|
||||||
self._bag_path = self.get_parameter('bag_path').value
|
self._bag_path = self.get_parameter('bag_path').value
|
||||||
self._topics_param = self.get_parameter('topics').value
|
topics_param = self.get_parameter('topics').value
|
||||||
|
# [''] means not specified — record all topics
|
||||||
|
self._topics_param: list[str] = [t for t in topics_param if t]
|
||||||
config_path = self.get_parameter('config_path').value
|
config_path = self.get_parameter('config_path').value
|
||||||
|
|
||||||
cfg = self._load_config(config_path)
|
cfg = self._load_config(config_path)
|
||||||
@@ -56,8 +54,11 @@ class IiwaTestRunner(Node):
|
|||||||
self._registered_topics: set[str] = set()
|
self._registered_topics: set[str] = set()
|
||||||
self._subs = []
|
self._subs = []
|
||||||
|
|
||||||
self._init_bag()
|
if self._bag_path:
|
||||||
self._init_subscribers()
|
self._init_bag()
|
||||||
|
self._init_subscribers()
|
||||||
|
else:
|
||||||
|
self.get_logger().info('bag_path not set — recording disabled')
|
||||||
|
|
||||||
# Config
|
# Config
|
||||||
def _load_config(self, config_path: str) -> dict:
|
def _load_config(self, config_path: str) -> dict:
|
||||||
@@ -68,6 +69,11 @@ class IiwaTestRunner(Node):
|
|||||||
|
|
||||||
# Bag files
|
# Bag files
|
||||||
def _init_bag(self):
|
def _init_bag(self):
|
||||||
|
bag_dir = Path(self._bag_path)
|
||||||
|
if bag_dir.exists():
|
||||||
|
shutil.rmtree(bag_dir)
|
||||||
|
self.get_logger().info(f'Removed existing bag at {self._bag_path}')
|
||||||
|
|
||||||
storage_opts = rosbag2_py.StorageOptions(uri=self._bag_path, storage_id='mcap')
|
storage_opts = rosbag2_py.StorageOptions(uri=self._bag_path, storage_id='mcap')
|
||||||
converter_opts = rosbag2_py.ConverterOptions(
|
converter_opts = rosbag2_py.ConverterOptions(
|
||||||
input_serialization_format='cdr',
|
input_serialization_format='cdr',
|
||||||
@@ -82,7 +88,11 @@ class IiwaTestRunner(Node):
|
|||||||
time.sleep(2.0)
|
time.sleep(2.0)
|
||||||
available = dict(self.get_topic_names_and_types())
|
available = dict(self.get_topic_names_and_types())
|
||||||
|
|
||||||
for topic in self._topics_param:
|
topics = self._topics_param if self._topics_param else list(available.keys())
|
||||||
|
if not self._topics_param:
|
||||||
|
self.get_logger().info(f'topics not set — recording all {len(topics)} available topics')
|
||||||
|
|
||||||
|
for topic in topics:
|
||||||
if topic not in available:
|
if topic not in available:
|
||||||
self.get_logger().warn(f'Topic {topic} not available, skipping')
|
self.get_logger().warn(f'Topic {topic} not available, skipping')
|
||||||
continue
|
continue
|
||||||
|
|||||||
Reference in New Issue
Block a user