Управления роботом Moveo в ROS ч.2 - инверсная кинематика с IKPy, управление роботом с помощью джойстика в Rviz

Обновился до ROS Noetic с поддержкой python3 😁 и, наконец, снова взялся за работу над пакетом moveo_ps3 для управления манипулятором Moveo с помощью джойстика PS3. 

 

 

В предыдущей части был сделан импорт робота с помощью URDF.

Теперь необходимо решить две задачи: 

• задавать с помощью контролера координаты цели в трехмерном пространстве, куда должен попасть захват манипулятора: положение и ориентация;

• решить обратную кинематическую задачу по преобразованию координат цели в углы между звеньями робота; 

• и здесь же подзадача - оценить ошибку позиционирования захвата относительно цели.

Для расчета кинематики решил использовать библиотеку IKPy, на ее стороне простота, универсальность, поддержка URDF.

Пакет moveo_ps3 для этого этапа можно скачать на GitHub. Пройдемся по его основным моментам. Скажу сразу, что без изменений остались только 3D модели (meshes). 

Начать нужно с файла URDF. На этом этапе используется дополненный moveo_ps3_extended.urdf, в котором добавлено два пустых звена, позволяющих определить новые системы координат (СК, frames): "world" - глобальная СК сцены и "end_effector" - СК захвата, она находится между его пальцами:

Положение цели задается с помощью СК "target". Относительное положение всех СК (глобальной, звеньев, цели) в пространстве сцены определяется в узле tf2 - специально предназначенном для доступа к положению любой СК относительно любой другой в заданный момент времени. Визуализация всей сцены в RViz выглядит следующим образом:

 

 

Активные СК перечислены в подразделе Frames раздела TF. Глобальная СК находится на платформе робота: относительно нее позиционированы базовое звено робота (base_link) и цель (target) - это показывают стрелки, направленные от них на СК "world".

Пакет запускается с помощью launch файла  moveo_ps3_ikpy_tf.launch:

$ roslaunch moveo_ps3 moveo_ps3_ikpy_tf.launch

Граф узлов пакета (rqt_graph) выглядит следующим образом:

Состояние осей и кнопок контроллера передается узлом /joy в топике /joy (подробней об этом можно посмотреть в документации joy_node, а также в одном из моих предыдущих постов про пакет urdf_test). 

В контроллере я использую следующие элементы:

Узел, отвечающий за интерпретацию сообщений контроллера - ps3_handler.py. Он с заданной частотой (10 Гц) читает сообщения в топике /joy, конвертирует их в сообщения PS3State.msg и отправляет в топик /ps3_state. При этом отслеживается id входящих сообщений, чтобы исключить их повторное чтение. Такая конструкция (loop mode + track id) позволяет как однозначно реагировать на нажатия кнопок, так и поддерживать движение цели если, например, "зажата" одна из осей. Узел построен на основе класса:

class PS3Handler:

'''Class for handling joystick messages, works in loop mode

Subscriber:

* /joy

Publisher:

* /ps3_state

'''

def __init__(self) -> None:

# meters

self.change_position = [0, 0, 0]

# euler angles, degrees

self.change_orientation = [0, 0, 0]

# true - pos, false - orient

self.switch_pos_orient = True

# speed / reset: 0 - pos, 1 - orient

self.speed = [0, 0]

self.reset = [False, False]

self.calc_active = True

self.calc_orient = True

# manipulator ctrl

self.move = False

self.gripper = False

# msg counters

self.in_msg_seq = 0

self.out_msg_seq = 0

# start node

self.run()

Обработчик входящих сообщений joy_collback() транслирует состояния элементов контроллера в переменные класса, откуда их берет для публикации паблишер pub_target_state(). При этом параметры change_position / orientation имеют смысл смещений цели, которые далее в узле /target_positioner с учетом скорости пересчитываются в глобальные координаты цели. Посмотреть содержимое сообщений в топике /ps3_state можно с помощью команды

$ rostopic echo /ps3_state

Еще из особенностей работы узла /ps3_handle: реакция на все кнопки, кроме, reset pos / orient сделана в виде триггера-защелки - их состояние переключается и удерживается. Состояния reset меняются однократно после нажатия кнопок, после чего сбрасываются. 

Далее по графу узлов, сообщения в топике /ps3_state поступают в три узла: /target_positioner, /target_marker и /moveo_ikpy.

Узел /target_positioner отвечает за пересчет смещений цели в их координаты в глобальной СК "world" - см. функцию ps3_state_callback():

self.position = list(map(lambda x, y: x + y*speed_pos, self.position, msg.change_position))

# Rotation - multiplication of current and received

euler_rot = list(map(lambda x: x*speed_orient, msg.change_orientation))

# http://docs.ros.org/en/jade/api/tf/html/python/transformations.html

# http://wiki.ros.org/tf2/Tutorials/Quaternions

q_curr = tf_trans.quaternion_from_euler(*np.radians(self.orientation))

q_rot = tf_trans.quaternion_from_euler(*np.radians(euler_rot))

q_res = tf_trans.quaternion_multiply(q_curr, q_rot)

self.orientation = np.degrees(tf_trans.euler_from_quaternion(q_res))

self.quaternion = q_res

Полученное перемещение просто суммируется с текущим, учитывая скорость.  С ориентацией несколько сложнее: она определяется как три угла Эйлера (отн. X - тангаж, отн. Y - крен, отн. Z - рыскание), при этом, мне хотелось сделать сферическое вращение, при котором в качестве осей вращения используются оси подвижной СК цели. Так, на мой взгляд, удобней поворачивать цель - всегда есть возможность  сделать поворот относительно какой-то одной из осей. Однако, есть и другие способы определить поворот, например, если переставить в функции quaternion_multiply() местами кватернионы, то вращение будет происходить относительно осей глобальной СК. Также при конвертации ушлов Эйлера в кватернион выбрать преобразование 'rXYZ' - вращающаяся СК, в которой поворот отн. осей X, Y рассчитывается в глобальной СК, а поворот отн. Z - в СК, вращающейся вместе с целью. Т.к. ось Z направлена вдоль пальцев захвата (его продольная ось), то таким образом можно задавать вращение захвата, независимо от того, как ориентирована цель. Однако, т.к. вращение захвата производится не в последнем суставе, а в предыдущем, то чтобы обеспечить такое вращение, необходимо, чтобы сразу два звена оказались на одной оси, что далеко не всегда достижимо.

Также в узле /target_positioner определена исходная позиция цели, выставляемая в начале работы и при получении команд reset:

def reset_pose(self, pos: bool=False, orient: bool=False) -> None:

'''Reset target position or/and orientation'''

if pos: self.position = [0., -0.22, 0.7956]

if orient: self.orientation = [0., 0., 0.]

Положение цели в глобальной СК, рассчитанное в узле /target_positioner передается  в узел /target_tf2_publisher, который публикует их в единое пространство СК - /tf2. Для этого используется топик /target_pose: координаты в нем передаются в метрах, а ориентация в виде кватерниона. Это позволяет просто транслировать их в /tf2 без дополнительных преобразований.

Оба узла в этой цепочке: /target_positioner и /target_tf2_publisher работают в режиме отклика (spin mode), генератором частоты для которого является публикация сообщений в топике /ps3_state (10 Гц). На данном этапе, параметры частоты работы узлов, а также названия СК задаются непосредственно в узлах, в будущем планирую вынести их в сервер параметров ROS, чтобы все значимые параметры были определены в launch-файле.

Последний узел, необходимый для создания цели - /target_marker, отвечающий за ее визуализацию в Rviz в виде маркера. Он построен на базе класса TargetMarker, в его переменных определены максимальные отклонения перемещения / ориентации, при которых считается, что захват не достиг цели.

class TargetMarker:

'''

Class for publishing target visualization marker depending transform error /target vs /end_effector,

works in spin mode

Subscriber:

* /tf

* /ps3_state

Publisher:

* /visualization_marker

'''

def __init__(self) -> None:

self.pos_error = 0

self.orient_error = 0

self.max_pos_error = 0.005 # 5 mm

self.max_orient_error = 0.5 # 0.5 degree

self.calc_active = True

self.calc_orient = True

self.base_frame = ''

self.gripper = False

# start node

self.tf_buffer = tf2_ros.Buffer()

self.run()

Относительное положение систем координат цели "target" и захвата "end_effector" узел получает из /tf2:

target_tf = self.tf_buffer.lookup_transform('end_effector', 'target', rospy.Time())

на основании чего и подсчитывается отклонение как норма векторов перемещения и вращения:

self.pos_error = np.linalg.norm((target_tf.transform.translation.x,

target_tf.transform.translation.y,

target_tf.transform.translation.z))

target_end_effector_rotation = (target_tf.transform.rotation.x,

target_tf.transform.rotation.y,

target_tf.transform.rotation.z,

target_tf.transform.rotation.w)

euler = tf_conversions.transformations.euler_from_quaternion(target_end_effector_rotation, axes='rxyz')

self.orient_error = np.linalg.norm(np.degrees(euler))

В зависимости от величины ошибки, а также от тех параметров, коорые узел получает в топике /ps3_state, принята следующая цветовая кодировка:

• зеленый - ошибки позиционирования (перемещение и вращение) не превышают допуска,
• оранжевый - ошибка перемещения в допуске, ошибка вращения больше допуска,
• синий - ошибка перемещения в допуске, ориентирование отключено,
• красный - ошибка перемещения больше допуска,
• серый - расчет кинематики отключен, перемещается только маркер.

Если захват свободен - маркер имеет вид сферы, если активирован - прямоугольника. 

Позиция маркера и его параметры публикуются в топике /visualization_marker, на который подписан Rviz (почему-то на графе эта связь отсутствует). Координаты маркера также берутся из /tf2 - и в этом его сила ☺.

Вторая ветка графа обеспечивает, собственно, расчет углов между звеньями робота. Ее элементы также завязаны на /tf2: расчет инверсной кинематики производится в узле /moveo_ikpy, который передает эти значения в узел /robot_state_publisher, по сути, выполняющий функции расчета прямой кинематики и публикации СК звеньев в /tf2. 

Рассматривая узел /moveo_ikpy, стоит подробней остановиться на двух моментах:

1. в нем сделана загрузка пути к urdf-файлу из сервера параметров 

urdf_file = rospy.get_param('/moveo_ikpy/model') 

2. расчет инверсной кинематики с помощью модуля ikpy

def calc_IK(self) -> None:

'''Inverse kinematic calculation'''

init_pos = self.chain_links

if self.calc_orient:

orientation_matrix = tf_conversions.transformations.quaternion_matrix(self.target_rotation)

self.chain_links = self.chain.inverse_kinematics(self.target_position, orientation_matrix[:-1, :-1],

orientation_mode='all', initial_position=init_pos)

else:

self.chain_links = self.chain.inverse_kinematics(self.target_position, initial_position=init_pos)

# exclude world, base_link and end_effector fixed joints and round for messaging

self.joint_angles = np.round(self.chain_links[2:-1],4)

В зависимости от параметра calc_orient рассчитывается либо полное позиционирование захвата, либо только совпадение с координатами цели (узел получает их из /tf2).  

Для того, чтобы движение манипулятора было плавным, в функцию inverse_kinematics() передается предыдущая позиция робота - initial_position. Дело в том, что данная функция является "оберткой" над основной расчетной функцией inverse_kinematic_optimization, в которой присутствует параметр "starting_nodes_angles (numpy.array) – The initial pose of your chain". Если его не задать, то при каждом обращении обратная кинематика будет рассчитываться от исходной точки (все углы = 0), что приводит к внезапным "скачкам", обусловленным кинематической неопределенностью - когда робот резко перескакивает в другую позицию, удовлетворяющую решению задачи.

Также необходимо обратить внимание на то, что ikpy при расчете использует все соединения (joints) в цепочке urdf, при этом в /robot_state_publisher необходимо передавать только значения подвижных.

На этом по данному этапу, пожалуй, все. Далее буду пробовать управлять уже самим манипулятором - посмотрим, как это будет выглядеть.

Посмотреть как выглядит пакет в действии - можно в этом видео:

Let's go design!