Клубника — хороший пример задачи, которая для человека выглядит элементарно, а для робота оказывается довольно сложной. Человек практически одновременно видит ягоду, оценивает её спелость, понимает, где она находится в пространстве, просовывает руку между листьями и другими ягодами, берёт плод с подходящим усилием и отделяет его, стараясь не повредить.
Для робота каждый из этих шагов превращается в отдельную инженерную задачу.
И это не только учебная проблема. Современные обзоры роботизированного сбора отмечают, что сбор свежей клубники по-прежнему сильно зависит от ручного труда, а сама ягода сложна для механизации из-за мягкости, неоднородного расположения и риска повреждения. Отдельная проблема — перекрытие ягод листьями, стеблями и соседними плодами.
Как собирают сейчас
Есть условно три уровня:
ручной сбор ↓механизация и помощь сборщику ↓автономный роботизированный сбор
При ручном сборе человек сам определяет зрелость, выбирает ягоду и выполняет манипуляцию. Есть и промежуточные системы: например, роботы могут не срывать ягоду, а возить лотки и тем самым сокращать непроизводительные перемещения работников. В одном полевом исследовании такой подход уменьшил непроизводительное время сборщиков примерно на 60% при соотношении один робот на трёх работников. Реальный сборщик имеет производительность около 8 кг/час в среднем за 12 смену.
А существуют уже и специализированные роботизированные системы. Например, Dogtooth Technologies описывает роботов для выращиваемой на столах клубники, которые перемещаются вдоль рядов, находят зрелые ягоды, собирают их и выполняют контроль качества. Компания заявляет для своего пятого поколения производительность до 200 кг в день.
Упрощаем задачу
Пока реализовать полный цикл с мобильной платформой мы не в силах, поэтому пробуем создать управление манипулятором для этой задачи.
Схема имеет следующий вид:
есть клубника 🍓 ↓ X, Y, Zманипулятор ↓подвести захват ↓сжать захват
На этом этапе система ещё не ищет клубнику камерой. Координаты точки задаются вручную. Это позволяет сначала разобраться с управлением манипулятором, а уже затем подключать компьютерное зрение.
Первый способ управления — непосредственно суставами
И вот здесь появляется первый наш код.
Манипулятор имеет шесть управляемых суставов (joint):
joint1joint2joint3joint4joint5joint6
Каждый joint — это угол, указанный в радианах.
При этом важно понимать, что поворот одного сустава влияет на положение всех звеньев, расположенных после него.
Например, если повернуть joint1, фактически относительно основания повернётся почти вся рука. Если изменить joint6, движение в основном затронет уже конечную часть манипулятора.
Поэтому положение захвата определяется сразу совокупностью значений всех шести суставов.
Отправляем положение суставов из Python
Для первого эксперимента я использовал следующий небольшой ROS2-скрипт:
import rclpyfrom rclpy.node import Nodefrom rclpy.action import ActionClientfrom trajectory_msgs.msg import JointTrajectory, JointTrajectoryPointfrom control_msgs.action import GripperCommanddef main(): rclpy.init() node = Node("joint") arm = node.create_publisher( JointTrajectory, "/RMC1/arm95/arm_joint_trajectory_controller/joint_trajectory", 10 ) gripper = ActionClient( node, GripperCommand, "/RMC1/arm95/gripper_controller/gripper_cmd" ) msg = JointTrajectory() msg.joint_names = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] point = JointTrajectoryPoint() point.positions = [0.1, 0.3, -0.3, 0.0, 0.0, 0.0] point.time_from_start.sec = 3 msg.points = [point] rclpy.spin_once(node, timeout_sec=1) arm.publish(msg) rclpy.spin_once(node, timeout_sec=3) gripper.wait_for_server() goal = GripperCommand.Goal() goal.command.position = 1.0 gripper.send_goal_async(goal) rclpy.spin_once(node, timeout_sec=1) rclpy.shutdown()if __name__ == "__main__": main()
Разберём эту программу подробно.
Подключаем ROS 2
Начинается программа с импортов:
import rclpyfrom rclpy.node import Nodefrom rclpy.action import ActionClient
rclpy — Python-библиотека ROS 2. Через неё программа может создавать ROS-ноды, публиковать сообщения, подписываться на топики и работать с actions.
Node понадобится для создания нашей собственной ноды:
node = Node("joint")
А ActionClient используется для управления захватом.
Следующие импорты относятся уже непосредственно к управлению манипулятором:
from trajectory_msgs.msg import JointTrajectoryfrom trajectory_msgs.msg import JointTrajectoryPoint
Здесь используются два типа сообщений.
JointTrajectory описывает траекторию движения суставов целиком.
JointTrajectoryPoint описывает отдельную точку этой траектории — то есть конкретный набор положений суставов в определённый момент времени.
Это различие станет понятнее чуть ниже.
Запускаем ROS2
В начале функции main() вызывается:
rclpy.init()
Этой командой инициализируется ROS 2 для нашей Python-программы.
Затем создаётся нода:
node = Node("joint")
Теперь наша программа появляется в ROS-графе как отдельный узел с именем joint.
Создаём канал управления рукой
Следующий фрагмент:
arm = node.create_publisher( JointTrajectory, "/arm_joint_trajectory_controller/joint_trajectory", 10)
создаёт publisher.
Publisher можно представить как передатчик сообщений.
Наша нода будет отправлять сообщения типа JointTrajectory в топик /arm_joint_trajectory_controller/joint_trajectory
Этот топик слушает контроллер манипулятора, то есть непосредственно двигателями Python-программа не управляет. Она сообщает контроллеру желаемое положение суставов, а уже контроллер выполняет необходимое движение.
Создаём сообщение JointTrajectory
Следующая строка:
msg = JointTrajectory()
создаёт пустое сообщение с траекторией.
Теперь необходимо сообщить контроллеру, какие суставы входят в эту траекторию.
Для этого записываем:
msg.joint_names = [ "joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]
Получается список из шести суставов.
Сам по себе этот список ещё не заставляет руку двигаться
Задаём положение руки
Теперь создаём точку траектории:
point = JointTrajectoryPoint()
Именно здесь появляется самая важная строка программы:
point.positions = [ 0.1, 0.3, -0.3, 0.0, 0.0, 0.0]
Эти числа соответствуют суставам, которые мы перечислили выше.
ROS сопоставляет два массива по порядку, при этом знаки имеют значение. Таким образом, строка:
point.positions = [0.1, 0.3, -0.3, 0.0, 0.0, 0.0]
является практически непосредственным описанием того, какую конфигурацию должна принять рука.
Почему это называется траекторией, если точка всего одна?
Название JointTrajectory сначала может немного запутать. В нашем примере мы фактически передаём только конечную точку, но ROS позволяет передавать несколько таких точек. Для каждой точки можно определить собственные положения суставов и время. Мы же пока используем максимально простой вариант.
Задаём время движения
Следующая строка:
point.time_from_start.sec = 3
говорит контроллеру, через какое время от начала траектории должна быть достигнута эта точка.
В нашем случае:
t = 0 секундтекущее положение ↓ движениеt = 3 секунды[0.1, 0.3, -0.3, 0, 0, 0]
Таким образом, мы задаём не только конечное положение суставов, но и временную характеристику движения.
Добавляем точку в траекторию
До этого момента point существовал отдельно от msg.
Теперь соединяем их:
msg.points = [point]
И сообщение JointTrajectory становится полностью сформированным.
Внутри него условно находится:
JointTrajectoryСуставы:joint1joint2joint3joint4joint5joint6 +Точка:[0.1, 0.3, -0.3, 0, 0, 0] +Время:3 секунды
Теперь это сообщение уже можно отправлять контроллеру.
Отправляем команду
Перед публикацией в тестовом коде вызывается:
rclpy.spin_once(node, timeout_sec=1)
Это даёт ноде возможность обработать события ROS2 перед отправкой команды.
После этого выполняется главное действие:
arm.publish(msg)
Сообщение отправляется в топик контроллера.
Именно в этот момент наша команда уходит из Python-программы:
point.positions │ ▼JointTrajectory │ ▼publish() │ ▼ROS 2 │ ▼joint trajectory controller │ ▼манипулятор
После публикации программа оставляет ноду активной ещё некоторое время:
rclpy.spin_once(node, timeout_sec=3)
В данном простом тесте это удобно, потому что мы задали движение продолжительностью три секунды.
Однако здесь есть важное упрощение: мы просто ждём три секунды. В полноценной программе лучше получать от контроллера информацию о фактическом завершении движения.
Теперь закрываем захват
До этого момента мы управляли только шестью суставами самой руки. Но для сбора клубники этого недостаточно. После того как рука подошла к ягоде, необходимо управлять ещё и захватом. Захват в нашей системе имеет собственный контроллер, поэтому для него создаётся отдельный клиент.
Обратите внимание на важное отличие:
Рука управляется через JointTrajectory, а захват — через GripperCommand. То есть программно это два отдельных исполнительных механизма.
Почему для захвата используется ActionClient?
Для руки в нашем простом примере мы просто публикуем сообщение:
arm.publish(msg)
Для захвата используется ROS2 Action.
Action подходит для команды, выполнение которой занимает некоторое время и для которой можно получить информацию о принятии и результате выполнения.
Сначала программа ждёт доступности контроллера:
gripper.wait_for_server()
Затем создаёт команду:
goal = GripperCommand.Goal()
И задаёт положение захвата:
goal.command.position = 0.0
Для используемой конфигурации:
0.04 │ └── захват открыт0.00 │ └── захват закрыт
Что в итоге делает вся программа?
Если убрать весь служебный код ROS2, алгоритм оказывается очень маленьким:
1. Создать ROS-ноду2. Задать: joint1 = 0.1 joint2 = 0.3 joint3 = -0.3 joint4 = 0 joint5 = 0 joint6 = 03. Дать руке 3 секунды на движение4. Закрыть захват
Именно поэтому управление через joint оказалось удобным первым этапом проекта.
Мы пока вообще не решаем сложную задачу: Где находится клубника и как автоматически подвести к ней захват? Вместо этого сначала проверяем более фундаментальные вещи.
Только после проверки этого уровня имеет смысл переходить к следующей задаче — определению положения ягоды и автоматическому расчёту необходимых движений манипулятора.
Это ещё не автономный сбор клубники, но уже необходимый нижний уровень будущей системы: мы научились программно задавать конфигурацию манипулятора и отдельно управлять его захватом.
Заключение
Разрабатываемая система представляет собой основу робота-манипулятора для автоматизации сбора.
На текущем этапе управление уже может осуществляться на уровне отдельных суставов, а наличие настроенного MoveIt позволяет перейти к планированию движения рабочего органа в декартовых координатах.
Следующим важным этапом является объединение системы технического зрения и манипулятора. Камера должна определять положение ягоды, после чего его координаты будут преобразовываться в систему координат робота и передаваться системе планирования движения.
В результате робот должен получить возможность самостоятельно подъехать к требуемой рабочей зоне, обнаружить объект, рассчитать движение манипулятора, выполнить захват и продолжить выполнение задания.
ссылка на оригинал статьи https://habr.com/ru/articles/1082770/