URDF. Управление движением в MoveIt!

Управление роботом-манипулятором требует решения прямой и/или обратной задач кинематики, т.е. определение положения рабочего инструмента при заданных параметрах суставов, либо вычисление углов/смещений по осям для обеспечения желаемых координат и ориентации инструмента. В общем случае, это нетривиальная задача. К счастью, фреймворк MoveIt! в системе ROS берёт эти расчёты на себя. Документацию по его использованию можно найти на официальном сайте. В этом посте я попытаюсь применить MoveIt! к разработанной ранее модели робота. 

Но сперва несколько слов о модели. Во-первых, я преобразовал её описание в xacro формат. С одной стороны, это позволяет изменять отдельные параметры, не переписывая целиком весь файл, с другой - проясняет модель, так как исчезают "магические числа", и становится понятно, как получен тот или иной коэффициент. Во вторых, появился четвёртый сустав, управляющий ориентацией рабочего инструмента. 

Для генерации пакета управления движением робота служит вспомогательная программа MoveIt Setup Assistant. Её можно запустить с помощью команды в терминале 

   roslaunch moveit_setup_assistant setup_assistant.launch 

В результате, откроется окно мастера настройки, в левой части которого перечислены предстоящие шаги. 

• Start. В этом окне вы находитесь при запуске программы, и от вас требуется выбрать режим работы (Choose mode). Так как мы хотим загрузить, а не редактировать, модель, нажимаем кнопку Create New MoveIt Configuration Package и выбираем файл с описанием модели (urdf или xacro). Завершаем загрузку нажатием Load Files.
• Self-Collisions. Генерирует матрицу параметров, позволяющих детектировать пересечение робота с самим собой. Жмём Generate Collision Matrix. 
• Virtual Joints. Виртуальные соединения используются для "крепления" модели к окружающему миру. Выбираем Add Virtual Joint, устанавливаем Virtual Joint Name: world2base; Child Link: world; Parent Frame Name: world; Joint Type: fixed. В данном случае можно было использовать base_link в качестве Child Link, чтобы избежать "тавтологии".
• Planning Groups. Для удобства работы с роботом отдельные элементы конструкции можно объединить в группы. Для манипулятора это могут быть "рука" и "инструмент". Выберем Add Group, укажем Group Name: body; Kinematic Solver: lma_kinematics_plugin/LMAKinematicsPlugin (для текущей модели данный "решатель" находит адекватный результат). Затем нажмите Add Joints и перенесите в правую часть суставы base_to_link1, link1_to_link2, link2_to_link3, link3_to_ee.  Полученный результат можно сохранить. Добавим ещё одну группу с именем tool, на этот раз используем Add Links и включим в её состав единственное звено ee, которое выполняет роль инструмента.
• Robot Poses. Заданная (начальная) конфигурация робота. Для добавления нажмите Add Pose и установите желаемые углы в суставах. 
• End Effectors. Выбор звена, отвечающего за положение рабочего инструмента. Нажмите Add End Effector, установите End Effector Name: tool; End Effector Group: tool; Parent Link: ee. 
• Passive Joints. Пассивные суставы обычно встречаются в параллельных конструкциях, в нашем случае оставляем незаполненным. 
• Author Information. Здесь можно указать своё имя и координаты для связи, если планируете где-то выложить результат. 
• Configuration Files. Выберите путь для сохранения, включая имя пакета (например scara_moveit_config). Можно также указать, какие файлы должны быть сгенерированы. После этого нажмите Generate Package. Поздавляю, вы настроили MoveIt!

 Чтобы посмотреть результат, выполните команду

   roslaunch scara_moveit_config demo.launch 

Если ошибок нет, должен открыться Rviz с моделью робота. Данная модель интерактивна, если тянуть за "шар" на последнем звене робота, манипулятор будет смещаться. Того же эффекта позволяют добиться разноцветные стрелки, а круги регулируют ориентацию рабочего инструмента. На этом возможности взаимодействия с моделью не заканчиваются. Подробнее с параметрами отображения и движения можно познакомиться в руководстве.

Рассмотрим некоторые возможности программного управления роботом, основанные на примере из руководства. Для этого используем интерфейс MoveIt! на языке Python, который будет заключен в класс MoveItScaraInterface. В конструкторе мы указываем родительский класс, инициализируем интерфейс moveit_commander, который осуществляет взаимодействие между программой и "решателем", а также копируем ссылки на несколько объектов для ускорения дальнейшего доступа. Результаты расчётов будут публиковаться по адресу "/move_group/display_planned_path".

   def __init__(self):

      # initialize
      super(MoveItScaraInterface, self).__init__()
      moveit_commander.roscpp_initialize(sys.argv)
      rospy.init_node("moveit_scara_interface", anonymous=True)

      # get objects
      self.robot = moveit_commander.RobotCommander()
      self.scene = moveit_commander.PlanningSceneInterface()
      self.group = moveit_commander.MoveGroupCommander("body")
      self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)

Простейшим видом движения является перемещение в пространстве конфигураций, при этом мы просто указываем роботу, какие значения в суставах нужно установить, с помощью метода go(цель). Флаг wait=True заставляет программу прекратить дальнейшую работу, пока цель не будет достигнута.

   def go_to_initial_state(self):
      group = self.group
      # prepare initial values
      goal = group.get_current_joint_values()
      goal[0] = -pi/3
      goal[1] = -pi/3
      goal[2] = 0
      goal[3] = 0
      # setup
      group.go(goal, wait=True)
      group.stop()

Движение в заданную точку (и достижение нужной ориентации) уже требуют вычислений с учётом геометрии робота. Функция движения осталась той же, но теперь она вызывается без аргумента, по-этому использует заданные координаты goal в декартовом пространстве.

   def go_to_pose(self):
      group = self.group
      # prepare position in cartesian space     
      goal = geometry_msgs.msg.Pose()     
      goal.orientation.w = 1.0        
      goal.position.x = 0.5
      goal.position.y = 0
      goal.position.z = 0.3
      group.set_pose_target(goal)
      # plan and execute    
      group.go(wait=True)     
      group.stop()
      group.clear_pose_targets()

Если нужно пройти через несколько точек, можно объединить их в список и просчитать перемещение с помощью compute_cartesian_path(путь, шаг_в_декартовом_пространстве, порог_в_пространстве_конфигураций). 

   def plan_cartesian_path(self, scale=1):
      group = self.group
      # prepare points
      waypoints = []
      wpose = group.get_current_pose().pose
      wpose.position.z += scale * 0.1     
      waypoints.append( copy.deepcopy(wpose) )
      wpose.position.y = 0.6
      wpose.position.x = 0
      wpose.orientation.z += pi
      waypoints.append( copy.deepcopy(wpose) )
      wpose.position.z -= scale * 0.3
      waypoints.append( copy.deepcopy(wpose) )
      # compute
      (plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0)
      return plan, fraction

Полный текст файла move_scara.py можно найти на bitbacket.org. Чтобы запустить движение роботом, откройте demo.launch, как было описано выше. Я рекомендую убрать в настройках галочки с Planning Request/Query Start State (чтобы отключить отображение планируемого положения) и Planned Path/Loop Animation (чтобы не было бесконечного движения из начальной точки в конечную). Теперь можно запустить выполнение программы, при этом перед каждым действием робот будет ждать подтверждения в виде нажатия клавиши Enter.