Skip to main content

Роботы

Для описания параметров звеньев написан класс core.robot.base.Link. Он хранит внутри себя следующие поля:

  • Путь к модели
  • Название
  • Матрица преобразования из СК мира в СК звена для неиерархичных звеньев (для удобства)
  • Тензор инерции звена
  • Координаты центра масс

Для работы с сочленениями реализовано два класса: core.robot.base.Joint и core.robot.base.JointParams.

Первый отвечает за преобразование угла поворота звена в матрицу перехода из системы координат (СК) предыдущего звена в СК следующего.

Второй хранит в себе неизменяемые параметры звена:

  • максимальное по модулю ускорение
  • максимальная по модулю скорость
  • максимальный угол
  • минимальный угол
  • индекс сочленения

Матрицы преобразования​

Матрица преобразования состоит из четырёх столбцов и четырёх строк

Чтобы получить координаты вектора V1V_1 в новой СК, нужно умножить матрицу преобразования T01T_{01} на его значение (V0V_0) в исходной СК:

V1=T01V0V_1 = T_{01}V_0

Т.к. каждое сочленение core.robot.base.Joint характеризуется осью вращения VaV_a и углом поворота θ\theta, то матрица преобразования M(Va,θ)M(V_a,\theta ) вычисляется по формуле:

M(Va,θ)=(cos⁡θ+(1−cos⁡θ)x2(1−cos⁡θ)xy−(sin⁡θ)z(1−cos⁡θ)xz+(sin⁡θ)y0(1−cos⁡θ)yx+(sin⁡θ)zcos⁡θ+(1−cos⁡θ)y2(1−cos⁡θ)yz−(sin⁡θ)x0(1−cos⁡θ)zx−(sin⁡θ)y(1−cos⁡θ)zy+(sin⁡θ)xcos⁡θ+(1−cos⁡θ)z200001)M(V_a,\theta )={\begin{pmatrix}\cos \theta +(1-\cos \theta )x^{2}&(1-\cos \theta )xy-(\sin \theta )z &(1-\cos \theta )xz+(\sin \theta )y&0\\ (1-\cos \theta )yx+(\sin \theta )z&\cos \theta +(1-\cos \theta ) y^{2}&(1-\cos \theta )yz-(\sin \theta )x&0\\ (1-\cos \theta )zx-(\sin \theta )y&(1-\cos \theta )zy+(\sin \theta )x&\cos \theta +( 1-\cos \theta )z^{2}&0\\ 0&0&0&1 \end{pmatrix}}

Для вычисления матрицы преобразования и её производных необходимо сначала задать угол поворота jointAngle и ось вращения axis, после чего вызвать метод 

/**
 * Получить матрицу преобразования сочленения
 * @return матрица преобразования сочленения
 */
Eigen::Matrix4d getTransformMatrix() { return parentTransform * getRotMatrix4x4(axis, jointAngle); }

parentTransform - это матрица преобразования из СК родительского сочленения в СК текущего. Также дополнительно хранится матрица преобразования из СК сочленения в СК следующего за ним звена linkTransform

Некоторые сочленения могут быть виртуальными, поэтому добавлен флаг

/**
 * Флаг, что сочленение является виртуальным и не связано со звеном
 */
bool isVirtual;

Также сочленение может быть связано со звеном, но при этом не иметь возможности менять угол поворота. Чаще всего это полезно при сложной геометрии звена робота. 

/**
 * Флаг, является ли сочленение фиксированным (без привода)
 */
bool isFixed;

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

∂M(Va,θ)∂θ=(sin⁡(θ)∗x2−sin⁡(θ)x∗y∗sin⁡(θ)−z∗cos⁡(θ)y∗cos⁡(θ)+x∗z∗sin⁡(θ)z∗cos⁡(θ)+x∗y∗sin⁡(θ)sin⁡(θ)∗y2−sin⁡(θ)y∗z∗sin⁡(θ)−x∗cos⁡(θ)x∗z∗sin⁡(θ)−y∗cos⁡(θ)x∗cos⁡(θ)+y∗z∗sin⁡(θ)sin⁡(θ)∗z2−sin⁡(θ))\frac{\partial M(V_a,\theta )}{\partial \theta}={\begin{pmatrix} \sin(\theta)*x^2 - \sin(\theta)& x*y*\sin(\theta) - z*\cos(\theta)& y*\cos(\theta) + x*z*\sin(\theta)\\ z*\cos(\theta) + x*y*\sin(\theta)& \sin(\theta)*y^2 - \sin(\theta)& y*z*\sin(\theta) - x*\cos(\theta)\\ x*z*\sin(\theta) - y*\cos(\theta)& x*\cos(\theta) + y*z*\sin(\theta)& \sin(\theta)*z^2 - \sin(\theta) \end{pmatrix}}
∂M(Va,θ)∂θ=(2∗x∗sin⁡(θ)y∗sin⁡(θ)z∗sin⁡(θ)y∗sin⁡(θ)0−cos⁡(θ)z∗sin⁡(θ)cos⁡(θ)0)\frac{\partial M(V_a,\theta )}{\partial \theta}={\begin{pmatrix} 2*x*\sin(\theta)& y*\sin(\theta)& z*\sin(\theta) \\ y*\sin(\theta)& 0& -\cos(\theta)\\ z*\sin(\theta)& \cos(\theta)& 0\\ \end{pmatrix}}

в классе Joint данная логика реализована в соответствующих методах:

/**
 * Получить первую производную матрицы преобразования сочленения
 * @return первая производную матрицы преобразования сочленения
 */
Eigen::Matrix4d getDiffTransformMatrix() { return parentTransform * getDiffRotMatrix4x4(axis, jointAngle); }

/**
 * Получить вторую производную матрицы преобразования сочленения
 * @return вторая производную матрицы преобразования сочленения
 */
Eigen::Matrix4d getDiff2TransformMatrix() { return parentTransform * getDiff2RotMatrix4x4(axis, jointAngle); }

Реализация методов getDiffRotMatrix4x4, getDiff2RotMatrix4x4 и других, необходимых для работы с матрицами, находится в core.misc.matrixMath

Иерархия классов​

Для работы с роботом как с объединением звеньев и сочленений написан базовый класс core.robot.base.BaseRobot.

У него прописаны все базовые методы для работы с роботом, но при этом он является абстрактным. Для его использования необходимо унаследовать новый класс от него и реализовать метод загрузки из файла:

/**
 * @brief загрузить параметры робота из файла
 *
 * Загрузить параметры робота из файла, нужно заполнить следующие поля:
 * _jointParams, _links, _nonHierarchicalLinks, path
 * @param path путь к файлу описания робота
*/
virtual void loadFromFile(std::string path) = 0;

Основным классом для работы с роботами является core.robot.URDFRobot. Он использует файлы расширения *.urdf. Подробнее с его описанием можно ознакомиться здесь.

Для чтения модели используется немного устаревшая библиотека urdf_reader без ROS (пакет project.urdf_reader). Более актуальную версию можно скачать здесь

Также для тестов был написан робот core.robot.DHRobot, использующий параметры Денавита-Хартенберга. Его описание получилось несколько громоздким. Это вызвано тем, что необходимо было согласовать матрицы преобразования urdf робота и робота с параметрами Денавита-Хартенберга.

Частные производные​

Матрицу преобразования можно вычислить, последовательно перемножив матрицы Ti(qi)T_{i}(q_i) преобразования, зависящие только от угла поворота ii-го звена qiq_i и матрицы перехода между звеньями PijP_{ij}:

M=P01T1(q1)P12T2(q2)P23....Pn−1nTn(qn)M = P_{01}T_{1}(q_1)P_{12}T_2(q_2)P_{23}....P_{n-1n}T_n(q_n)

Если взять от этой формулы частную производную по qiq_i, то от этой координаты зависит только соответствующая матричная функция Ti(qi)T_{i}(q_i), все остальные множители при дифференцировании можно воспринимать как постоянные.

Поэтому при вычислении Mi′M'_i достаточно просто заменить ii-ю матричную функцию на её производную:

Mi′=P01T1(q1)P12T2(q2)P23....Pi−1i∂Ti(qi)∂qiPii+1....Pn−1nTn(qn)M'_i = P_{01}T_{1}(q_1)P_{12}T_2(q_2)P_{23}....P_{i-1i}\frac{\partial T_i(q_i)}{\partial q_i}P_{ii+1}....P_{n-1n}T_n(q_n)

Для частной производной второго порядка в случае i≠ji\neq j заменить ii-ю и jj-ю матричные функции её производными (не теряя общности, будем считать, что i<ji<j):

Mij′′=P01T1(q1)P12T2(q2)P23....Pi−1i∂Ti(qi)∂qiPii+1....Pj−1j∂Tj(qj)∂qjPjj+1....Pn−1nTn(qn)M''_{ij} = P_{01}T_{1}(q_1)P_{12}T_2(q_2)P_{23}....P_{i-1i}\frac{\partial T_i(q_i)}{\partial q_i}P_{ii+1}.... P_{j-1j}\frac{\partial T_j(q_j)}{\partial q_j}P_{jj+1}....P_{n-1n}T_n(q_n)

Если i=ji=j, то необходимо заменить только ii-ю матричную функцию её второй производной:

Mii′′=P01T1(q1)P12T2(q2)P23....Pi−1i∂2Ti(qi)∂qi2Pii+1....Pn−1nTn(qn)M''_{ii} = P_{01}T_{1}(q_1)P_{12}T_2(q_2)P_{23}....P_{i-1i}\frac{\partial^2 T_i(q_i)}{\partial q_i^2}P_{ii+1}....P_{n-1n}T_n(q_n)

Чтобы вычислить производную по матрице преобразования того или иного звена, достаточно найти производную матрицы поворота (смещение закладывается в матрицах Pkk+1P_{kk+1}).

Поворот каждого звена описывается осью [xi, yi, zi][x_i,~y_i,~z_i] и углом поворота qiq_i вокруг неё. Тогда можно матрицу преобразования Ti(qi)T_i(q_i) можно найти по формуле:

Ti(qi)=[(1−cos⁡(qi))xi2+cos⁡(qi)−zisin⁡(qi)−xiyi(cos⁡(qi)−1)yisin⁡(qi)−xizi(cos⁡(qi)−1)0zisin⁡(qi)−xiyi(cos⁡(qi)−1)(1−cos⁡(qi))yi2+cos⁡(qi)−xisin⁡(qi)−yizi(cos⁡(qi)−1)0−yisin⁡(qi)−xizi(cos⁡(qi)−1)xisin⁡(qi)−yizi(cos⁡(qi)−1)(1−cos⁡(qi))zi2+cos⁡(qi)00001]\small T_i(q_i)=\begin{bmatrix} (1 - \cos(q_i))x_i^2 + \cos(q_i)& - z_i\sin(q_i) - x_iy_i(\cos(q_i) - 1)& y_i\sin(q_i) - x_iz_i(\cos(q_i) - 1) & 0\\ z_i\sin(q_i) - x_iy_i(\cos(q_i) - 1)& (1 - \cos(q_i))y_i^2 + \cos(q_i)& - x_i\sin(q_i) - y_iz_i(\cos(q_i) - 1) &0\\ - y_i\sin(q_i) - x_iz_i(\cos(q_i) - 1)& x_i\sin(q_i) - y_iz_i(\cos(q_i) - 1)& (1 - \cos(q_i))z_i^2 + \cos(q_i)&0\\ 0&0&0&1 \end{bmatrix}
∂Ti(qi)∂qi=[sin⁡(qi)xi2−sin⁡(qi)xiyisin⁡(qi)−zicos⁡(qi)yicos(qi)+xizisin⁡(qi)0zicos⁡(qi)+xiyisin⁡(qi)sin⁡(qi)yi2−sin⁡(qi)yizisin⁡(qi)−xicos⁡(qi)0xizisin⁡(qi)−yicos(qi)xicos⁡(qi)+yizisin⁡(qi)sin⁡(qi)zi2−sin⁡(qi)00000]\small \frac{\partial T_i(q_i)}{\partial q_i}=\begin{bmatrix} \sin(q_i)x_i^2 - \sin(q_i)& x_iy_i\sin(q_i) - z_i\cos(q_i)& y_icos(q_i) + x_iz_i\sin(q_i)&0\\ z_i\cos(q_i) + x_iy_i\sin(q_i)& \sin(q_i)y_i^2 - \sin(q_i)& y_iz_i\sin(q_i) - x_i\cos(q_i)&0\\ x_iz_i\sin(q_i) - y_icos(q_i)& x_i\cos(q_i) + y_iz_i\sin(q_i)& \sin(q_i)z_i^2 - \sin(q_i)&0\\ 0&0&0&0 \end{bmatrix}
∂2Ti(qi)∂qi2=[2xisin⁡(qi)yisin⁡(qi)zisin⁡(qi)0yisin⁡(qi)0−cos⁡(qi)0zisin⁡(qi)cos⁡(qi)000000]\small \frac{\partial^2 T_i(q_i)}{\partial q_i^2}=\begin{bmatrix} 2x_i\sin(q_i)& y_i\sin(q_i)& z_i\sin(q_i)&0\\ y_i\sin(q_i)& 0& -\cos(q_i)&0\\ z_i\sin(q_i)& \cos(q_i)& 0&0\\ 0&0&0&0 \end{bmatrix}