Для управления манипулятором нужно использовать специально разработанное API (интерфейс программирования приложения). Ниже представлено подробнее описание.
Содержание
Установка библиотек
MotionCore API или pip install motorcortex-python
Robot API или pip install motorcortex-robot-control-python
NNG API или pip install pynng
Protobuf или pip install protobuf==3.20.3
Также можете установить все библиотеки через pip install -r /path/to/requirements.txt
WARNING
API поддерживает версию protobuf 3.20.x
После установки в вашем окружении будут доступны библиотеки для импорта:
python
import motorcortex
from robot_control.motion_program import Waypoint, MotionProgram, PoseTransformer
from robot_control.robot_command import RobotCommand
from robot_control.system_defs import InterpreterStates, States, ModeCommands, StateEvents, Modes, InterpreterEvents, MotionGeneratorStatesПодключение к манипулятору и вход в систему
Манипулятор поддерживает WebSocket Security (wss), который требует наличия сертификата (открытого ключа) на клиенте, чтобы он мог подключиться. В общем случае каждое серверное приложение должно иметь свой собственный сертификат, чтобы доступ к нему можно было ограничить только теми клиентами, у которых установлена копия правильного сертификата.
Установите сертификат, соответствующий сертификату вашего сервера (в примере ниже это «mcx.cert.pem»), на ваш клиент. При подключении этот сертификат должен быть загружен.
python
parameter_tree = motorcortex.ParameterTree()
try:
req, sub = motorcortex.connect("wss://192.168.2.100:5568:5567", motorcortex.MessageTypes(), parameter_tree,
certificate="mcx.cert.pem", timeout_ms=1000,
login="", password="")
tree = parameter_tree.getParameterTree()
print(f"Parameters: {tree}")
except RuntimeError as err:
print(err)В данном коде:
IP-адрес манипулятора “192.168.2.100” может быть заменен на “localhost”, если исполняемый файл запускается с контроллера манипулятора
mcx.cert.pem - путь к сертификату
Файл mcx.cert.pem должен быть в корневой папке проекта, рядом с файлом запускаемого кода (или необходимо указывать путь к файлу вместо имени в программе).
WARNING
Если при подключении к манипулятору возникает ошибка “Entry not found”, проверьте:
- Введенный ip-адрес манипулятора
- Введенный порт манипулятора
- Путь к сертификату
WARNING
Если при подключении к манипулятору возникает ошибка “ImportError: cannot import name 'builder' from 'google.protobuf.internal' “, скачайте файл builder.py и перенесите в библиотеку google по пути ~/.local/lib/python3.10/site-packages/google/protobuf/internal
Получить/задать значение переменной
Чтобы получить значение определенного параметра с сервера, используйте:
python
print(f"nget_param_reply_msg: {req.getParameter('path/to/variable').get()}")Значение хранится в файле get_param_reply_msg.value, который всегда представляет собой кортеж значений, даже если значение является скаляром (доступ к которому можно получить через get_param_reply_msg.value[0]).
Чтобы задать скалярное значение, используйте:
python
print(f"nset_param_reply_msg: {req.setParameter('path/to/variable', True).get()}")Чтобы задать массив значений, используйте:
python
req.setParameter('path/to/array', [1, 2, 3, 4]).get()Для одновременной настройки нескольких параметров:
python
paramlist_to_set = [{ path: 'root/Control/dummyBool', value: False }, { path: 'root/Control/dummyDoubleArray6', value: [4, 3, 2, 1] }]
req.setParameterList(paramlist_to_set).get()Подписка на поток данных
Для получения непрерывного потока значений для набора параметров можно создать подписку. Клиентское приложение может объединить нужные параметры в группу. Вся группа отправляется сервером в одном пакете, поэтому все сигналы в группе синхронизируются.
Клиент может создать несколько подписок на разные группы, например, с разной скоростью передачи данных (задав параметр «делитель» для разных групп).
Имя группы разделяется между клиентами, и группа публикуется сервером один раз для всех клиентов, имеющих подписку на группу с таким именем. Таким образом, связь может легко масштабироваться на большое количество клиентов.
Если имя группы уже существует на сервере, а другой клиент запрашивает подписку на набор сигналов с тем же именем группы, сервер добавляет в группу все запрошенные сигналы, которых еще нет в группе. Другие клиенты, использующие то же имя группы, также получат дополнительные сигналы.
Python-библиотека MotionCore позволяет удобно добавлять подписки и обрабатывать полученные данные. С каждой подпиской связана функция обратного вызова, в которой данные могут быть обработаны и сохранены. При создании подписки также создается новый подпроцесс, который обрабатывает прием данных асинхронно с основным приложением.
Ниже приведен пример создания и обработки подписки.
python
import motorcortex
import time
# Определите функцию обратного вызова, которая будет вызываться каждый раз,
# когда будет получено сообщение
def message_received(parameters):
for cnt in range(0, len(parameters)):
param = parameters[cnt]
timestamp = param.timestamp.sec + param.timestamp.nsec * 1e-9
value = param.value
# выведите временную метку и значение; сначала преобразуйте значение в строку,
# чтобы не проверять все типы перед печатью
print(f"Notify: {timestamp}, {value}")
def main():
parameter_tree = motorcortex.ParameterTree()
# Открыть запрос и подписать соединение
try:
req, sub = motorcortex.connect("wss://192.168.2.100:5568:5567",
motorcortex.MessageTypes(), parameter_tree,
certificate="mcx.cert.pem", timeout_ms=1000,
login="", password="")
except RuntimeError as err:
print(err)
exit()
paths = ['root/Control_task/actual_cycle_max']
# определите делитель частоты, который указывает серверу публиковать только каждый
# n-ый образец. Это зависит от частоты обновления издателя.
divider = 100
# подписаться и ждать ответа с таймаутом в 10 секунд
subscription = sub.subscribe(paths, 'group1', divider)
# получить ответ от сервера
is_subscribed = subscription.get()
# статус и оформление подписки на печать
if (is_subscribed is not None) and (is_subscribed.status == motorcortex.OK):
print(f"Subscription successful, layout: {subscription.layout()}")
else:
print(f"Subscription failed, do your paths exist? \npaths: {paths}")
sub.close()
exit()
# установить функцию обратного вызова, которая обрабатывает полученные данные
# Обратите внимание, что это неблокирующий вызов, запускающий новый поток,
# который обрабатывает сообщения. Вы должны поддерживать приложение в рабочем
# состоянии до тех пор, пока вам необходимо получать сообщения
subscription.notify(message_received)
# подписка на опрос
for i in range(100):
value = subscription.read()
if value:
print(f"Polling, timestamp: {value[0].timestamp} value: {value[0].value}")
time.sleep(1)
# закройте подписку после завершения работы
sub.close()
if __name__ == "__main__":
main()TIP
При тестировании с несколькими пользователями в одном приложении обязательно используйте разные имена групп, чтобы избежать путаницы между пользователями.
Восстановление потерянного соединения
Библиотека автоматически обрабатывает временные потери соединения. Клиенту не нужно справляться с этим. Однако если сервер будет перезапущен, клиент не сможет автоматически переподключиться, поскольку сервер создал новый идентификатор сессии. В этом случае клиент может сбросить старое соединение, создать новое и снова войти в систему.
Закрытие соединения
Чтобы закрыть соединение, используйте:
python
req.close()Взаимодействие с манипулятором
Класс RobotCommand из библиотеки robot_control.robot_command представляет собой контроль состояния манипулятора.
Для того, чтобы инициализировать объект класса, необходимо задать ссылку на экземпляр запроса, возвращаемый из метода motorcortex.connect, и объект класса MessageTypes для работы с типами данных.
Пример кода для инициализации класса манипулятора:
python
import motorcortex
from robot_control.motion_program import Waypoint, MotionProgram, PoseTransformer
from robot_control.robot_command import RobotCommand
from robot_control.system_defs import InterpreterStates, States, ModeCommands, StateEvents, Modes, InterpreterEvents, MotionGeneratorStates
parameter_tree = motorcortex.ParameterTree()
messageTypes = motorcortex.MessageTypes()
try:
req, sub = motorcortex.connect("wss://192.168.56.101:5568:5567", messageTypes, parameter_tree,
certificate="../mcx.cert.pem", timeout_ms=1000,
login="", password="")
tree = parameter_tree.getParameterTree()
print(f"Parameters: {tree}")
robot = RobotCommand(req, messageTypes)
except RuntimeError as err:
print(err)| Класс RobotCommand | | :----------------: | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | | Метод | Описание | | off | Переключение робота в выключенное состояние | | disengage | Выключение моторов (Swith off) | | engage | Включение моторов (Switch on) | | manualCartMode | Переключение робота на ручное декартово движение | | manualJointMode | Переключение робота на ручное движение суставов | | semiAutoMode | Переключите робота в полуавтоматический режим. Полуавтомат перемещает руку к цели, когда пользователь удерживает кнопку. Полуавтоматический режим активен, например, во время перехода к началу программы | | toolTipOffset | Обновление смещения рабочего инструмента | | moveToPoint | Переместите руку в заданную позу по суставам | | moveToStart | Переместить манипулятор к началу программы | | play | Воспроизвести программу | | pause | Приостановить программу | | stop | Остановить программу | | reset | Остановить программу и очистить буфер интерпретатора | | getState | Возвращает фактическое состояние интерпретатора |
Статусы манипулятора
Для получения статусов манипулятора используется модуль system_defs из библиотеки robot_control
В нем имеется:
| States | Список состояний машины робота |
|---|---|
| StateEvents | Список состояний событий машины робота |
| ModeCommands | Список событий робота в режиме машины |
| Modes | Список состояний робота в режиме машины |
| InterpreterStates | Список состояний интерпретатора |
| InterpreterEvents | Список событий интерпретатора |
| MotionGeneratorStates | Список состояний генератора движения |
| FrameTypes | Список возможных кадров концевого эффектора робота |
python
motion_program = MotionProgram(req, messageTypes)
motion_program.addMoveJ(Waypoint([radians(0), radians(0), radians(90), radians(0), radians(90), radians(0)]), 0.05, 0.1)
if robot.play(wait_time=0.25) is InterpreterStates.MOTION_NOT_ALLOWED_S.value:
if robot.moveToStart(200):
print("Robot move to start position")
else:
print('Failed to move to the start position')
motion_program.send("move_to_start_point").get()
self.__robot.play(wait_time=0.25)Составление программы движения
Для программирования траектории движения робота используется модуль motion_program из библиотеки robot_control
Данный модуль содержит 3 класса Waypoint, MotionProgram, PoseTransformer
Waypoint - Класс представляет путевую точку траектории движения
Аргументы:
- pose(list(double)): поза в декартовом или совместном пространстве
- smoothing_factor(double): коэффициент сглаживания путевой точки в диапазоне [0..1]
- next_segment_velocity_factor(double): коэффициент скорости сегмента в диапазоне [0..1]
python
from math import *
Waypoint([radians(0), radians(0), radians(90), radians(0), radians(90), radians(0)]) #joint
Waypoint([0.24, -0.135, 0.24, pi/2, 0.0, pi]) #cart
#"Г" образное положение манипулятораWARNING
При движении в:
- MoveJ - значения задаются в радианах
- MoveL, MoveC - значения задаются в метрах
PoseTransformer - Преобразование декартовых координат в углы суставов и наоборот
Аргументы:
- req(motorcortex.Request): ссылка на экземпляр запроса
- motorcortex_types(motorcortex.MessageTypes): ссылка на экземпляр MessageTypes
| Метод | Описание |
|---|---|
| calcCartToJointPose | Преобразование ОЗК в ПЗК |
| calcJointToCartPose | Преобразование ПЗК в ОЗК |
python
robot_PoseTransformer = PoseTransformer(req, messageTypes)
actual_tool_pose = req.getParameter('root/ManipulatorControl/manipulatorToolPoseActual').get().value
actual_joint_pose = req.getParameter('root/ManipulatorControl/jointPositionsActual').get().value
get_new_joint_list = robot_PoseTransformer.calcCartToJointPose([0.3, -0.191, 0.7, pi / 2, 0.0, pi], actual_joint_pose)
get_new_cart_list = robot_PoseTransformer.calcJointToCartPose([radians(0), radians(0), radians(90), radians(0), radians(90), radians(0)], actual_tool_pose)
print(get_new_cart_list.jointtocartlist[0].cartpose)
print(get_new_joint_list.carttojointlist[0].jointpose)MotionProgram - Класс представляет программу движения манипулятора
Аргументы:
- req(motorcortex.Request): ссылка на экземпляр запроса
- motorcortex_types(motorcortex.MessageTypes): ссылка на экземпляр MessageTypes
| Метод | Описание |
|---|---|
| clear | Очищает буфер программы |
| addMoveC | Добавляет в программу команду MoveC (линейное движение по окружности) |
| addMoveL | Добавляет в программу команду MoveL (линейное движение) |
| addMoveJ | Добавляет в программу команду MoveJ (движение по суставам) |
| addComposedCartMove | Добавить составное декартово перемещение MoveL + MoveC |
| addWait | Добавляет задержку в программу |
| generateMessage | Генерирует сообщение |
| send | Отправляет программу на манипулятор |
INFO
При движение по окружности MoveC необходимо задавать минимум 3 точки
python
import motorcortex
from robot_control.motion_program import Waypoint, MotionProgram, PoseTransformer
from robot_control.robot_command import RobotCommand
from robot_control.system_defs import InterpreterStates, States, ModeCommands, StateEvents, Modes, InterpreterEvents, MotionGeneratorStates
from math import *
parameter_tree = motorcortex.ParameterTree()
messageTypes = motorcortex.MessageTypes()
try:
req, sub = motorcortex.connect("wss://192.168.2.100:5568:5567", messageTypes, parameter_tree,
certificate="mcx.cert.pem", timeout_ms=1000,
login="", password="")
robot = RobotCommand(req, messageTypes)
robot_PoseTransformer = PoseTransformer(req, messageTypes)
robot.engage()
motion_program = MotionProgram(req, messageTypes)
motion_program.addMoveJ([Waypoint([radians(0), radians(0), radians(90), radians(0), radians(90), radians(0)])], 0.01, 0.1)
motion_program.send("move_to_start_point").get()
if robot.play(wait_time=0.25) is InterpreterStates.MOTION_NOT_ALLOWED_S.value:
if robot.moveToStart(200):
print("Robot move to start position")
else:
print('Failed to move to the start position')
motion_program.addMoveL([Waypoint([0.24, -0.135, 0.24, pi/2, 0.0, pi]),
Waypoint([0.44, -0.135, 0.24, pi/2, 0.0, pi]),
Waypoint([0.44, 0.135, 0.24, pi/2, 0.0, pi]),
Waypoint([0.24, 0.135, 0.24, pi/2, 0.0, pi]),
Waypoint([0.24, -0.135, 0.24, pi/2, 0.0, pi])], velocity=0.1)
motion_program.addMoveJ([Waypoint([radians(0), radians(0), radians(90), radians(0), radians(90), radians(0)])])
motion_program.send("rectangular_movement").get()
while not (robot.play(wait_time=0.25) is InterpreterStates.PROGRAM_IS_DONE.value):
print("Manipulator moved")
else:
print("Manipulator done programm")
req.close()
sub.close()
except RuntimeError as err:
print(err)Описание параметров
| Параметр | Тип | Описание |
|---|---|---|
| root/ManipulatorControl/hostInToolVelocity | float array[6] | Задает скорость перемещения манипулятора в декартовом ространстве в cartesian mode, имеет 6 каналов (3 линейные координаты (xyz), 3 угла Эйлера) |
| root/ManipulatorControl/jointPositionsActual | float array[6] | Текущие положения степеней подвижности в радианах |
| root/ManipulatorControl/manipulatorToolPoseActual | float array[6] | Текущее положение рабочего инструмента в декартовом пространстве [x, y, z, Rz, Ry, Rx] |
| root/ManipulatorControl/runSpeedFactor | float | Cкорость перемещения манипулятора в процентах от максимальной, диапазон изменения [0.0, 1.0] |
Полезный совет
Каждый метод имеет описание при наведении мыши на нее.
requirements
motorcortex-python>=0.23.3 motorcortex-robot-control-python>=3.1.4 pypng>=0.8.1 protobuf==3.20.3