3.2 写一个UR机器人运动学库
作者：互联网
本博文属于工程机械臂末端柔顺控制（Ros+Gazebo仿真实现）
注：本文参考文献忘了，参考的是一篇中国学者发表的一篇关于和UR构型一致的6自由度机械臂求逆解规避掉第六个关节可能由于奇异构型无法求解的问题。
0 引言
末端柔顺控制必然牵扯到机械臂逆运动学求解。
注：由于本文涉及到矩阵运算，需要事先下载好Eigen矩阵运算库，可自行搜索如何安装。
1 工程目录
RoboticsLib
bin(存放可执行文件)
include(存放头文件)
Commom.h
DH_config.h
FrameTransform.h
Kinematics.h
src(存放源代码)
FrameTransform.cpp
Kinematics.cpp
test.cpp
lib(存放生成的库文件)
build(编译生成的中间文件)
CMakeLists.txt2 头文件
（1）Commom.h
#pragma once
#include <Eigen/Core>
typedef Eigen::Matrix<double, 6, 1> JointsState;
typedef Eigen::Matrix<double, 3, 1> Points;
typedef Eigen::Matrix<double, 4, 4> TransMat;
typedef Eigen::Matrix<double, 3, 3> RotMat;主要给后续需要用到的一些矩阵取别名。
JointState: 表示关节状态，一共6个关节，为6行1列矩阵；
Points：表示一个点，3行1列矩阵；
TransMat: 坐标系变换齐次矩阵，4行4列矩阵；
RotMat: 坐标系变换的旋转矩阵，3行3列。
上述定义的矩阵后续不一定全部被使用。
（2）DH_config.h
#pragma once
#include <vector>
#include <cmath>
namespace UR5_DH
{
std::vector<double> a = {0, -0.425, -0.39225, 0, 0, 0};
std::vector<double> alpha = {M_PI / 2, 0, 0, M_PI / 2, -1 * M_PI / 2, 0};
std::vector<double> d = {0.089459, 0, 0, 0.10915, 0.09465, 0.0823};
}本文档定义了UR5机器人的DH参数，后续如有需要，与UR系列相同构型的机械臂的DH参数也可以在此定义，后续解算运动学的时候可直接传入DH参数。
（3）FrameTransform.h
/* The API of Frame transformation */
#pragma once
#include <Eigen/Core>
#include "Common.h"
namespace FT
{
TransMat rotx(double angle);
TransMat roty(double angle);
TransMat rotz(double angle);
TransMat trans(double x, double y, double z);
}声明了几个函数，主要是求解沿着不同坐标轴旋转或平移时的齐次变换矩阵。
（4）Kinematics.h
#pragma once
#include <vector>
#include "Common.h"
class Kinematics
{
public:
/* a, alpha and d are the DH parameters */
Kinematics(std::vector<double> a, std::vector<double> alpha, std::vector<double> d);
public:
std::vector<double> a;
std::vector<double> alpha;
std::vector<double> d;
public:
bool FK(const JointsState &joints_angle, TransMat &mat);
bool IK(const TransMat &mat, std::vector<JointsState> &inverse_solutions);
};声明了一个运动学类，结构比较简单，成员变量为DH参数，两个成员函数分别计算运动学正解和逆解。
注意：两个成员函数都是返回bool值，计算成功为true，正解FK函数计算结果保存在TransMat &mat变量中，逆解IK计算结果保存在std::vector<JointsState> &inverse_solutions变量中。
此外，逆解一共有8组解，每一组解均为JointState，即6行1列的矩阵，因此使用std::vector<JointsState>来保存结果。
3 源代码
（1）FrameTransform.cpp
对应上述的FrameTransform.h
#include "FrameTransform.h"
#include <cmath>
TransMat FT::rotx(double angle)
{
TransMat mat;
mat << 1, 0, 0, 0,
0, cos(angle), -1 * sin(angle), 0,
0, sin(angle), cos(angle), 0,
0, 0, 0, 1;
return mat;
}
TransMat FT::roty(double angle)
{
TransMat mat;
mat << cos(angle), 0, sin(angle), 0,
0, 1, 0, 0,
-1 * sin(angle), 0, cos(angle), 0,
0, 0, 0, 1;
return mat;
}
TransMat FT::rotz(double angle)
{
TransMat mat;
mat << cos(angle), -1 * sin(angle), 0, 0,
sin(angle), cos(angle), 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
return mat;
}
TransMat FT::trans(double x, double y, double z)
{
TransMat mat;
mat << 1, 0, 0, x,
0, 1, 0, y,
0, 0, 1, z,
0, 0, 0, 1;
return mat;
}比较简单，求解就是坐标系不同的变换相应的齐次变换矩阵。
（2）Kinematics.cpp
#include "Kinematics.h"
#include "FrameTransform.h"
#include <iostream>
#include <cmath>
void limit_angle(Eigen::Matrix<double, 8, 1> &angles)
{
for (int i = 0; i <= 7;i++)
{
while(true)
{
if(angles[i] < -1 * M_PI)
{
angles[i] += 2 * M_PI;
}
else if(angles[i] > M_PI)
{
angles[i] -= 2 * M_PI;
}
else
{
break;
}
}
}
}
Kinematics::Kinematics(std::vector<double> a, std::vector<double> alpha, std::vector<double> d)
{
this->a = a;
this->alpha = alpha;
this->d = d;
}
bool Kinematics::FK(const JointsState &angles, TransMat &mat)
{
try
{
TransMat T12 = FT::rotz(angles[0]) * FT::trans(0, 0, this->d[0]) * FT::rotx(this->alpha[0])
* FT::trans(this->a[0], 0, 0);
TransMat T23 = FT::rotz(angles[1]) * FT::trans(0, 0, this->d[1]) * FT::rotx(this->alpha[1])
* FT::trans(this->a[1], 0, 0);
TransMat T34 = FT::rotz(angles[2]) * FT::trans(0, 0, this->d[2]) * FT::rotx(this->alpha[2])
* FT::trans(this->a[2], 0, 0);
TransMat T45 = FT::rotz(angles[3]) * FT::trans(0, 0, this->d[3]) * FT::rotx(this->alpha[3])
* FT::trans(this->a[3], 0, 0);
TransMat T56 = FT::rotz(angles[4]) * FT::trans(0, 0, this->d[4]) * FT::rotx(this->alpha[4])
* FT::trans(this->a[4], 0, 0);
TransMat T67 = FT::rotz(angles[5]) * FT::trans(0, 0, this->d[5]) * FT::rotx(this->alpha[5])
* FT::trans(this->a[5], 0, 0);
mat = T12 * T23 * T34 * T45 * T56 * T67;
return true;
}
catch(...)
{
std::cerr << "Forward kinematics solution failed"
<< "\n";
return false;
}
}
bool Kinematics::IK(const TransMat &mat, std::vector<JointsState> &inverse_solutions)
{
try
{
double nx = mat(0, 0);
double ny = mat(1, 0);
double nz = mat(2, 0);
double ox = mat(0, 1);
double oy = mat(1, 1);
double oz = mat(2, 1);
double ax = mat(0, 2);
double ay = mat(1, 2);
double az = mat(2, 2);
double px = mat(0, 3);
double py = mat(1, 3);
double pz = mat(2, 3);
Eigen::Matrix<double, 8, 1> theta1, theta2, theta3,
theta4, theta5, theta6;
/* ----------theta1---------- */
double A1 = px - ax * this->d[5];
double B1 = py - ay * this->d[5];
double X = sqrt(pow(A1, 2) + pow(B1, 2) - pow(this->d[3], 2));
theta1[0] = atan2(this->d[3], X) + atan2(B1, A1);
theta1[1] = theta1[2] = theta1[3] = theta1[0];
theta1[4] = atan2(this->d[3], -1 * X) + atan2(B1, A1);
theta1[5] = theta1[6] = theta1[7] = theta1[4];
/* ----------theta6---------- */
double A6 = ny * cos(theta1[0]) - nx * sin(theta1[0]);
double B6 = oy * cos(theta1[0]) - ox * sin(theta1[0]);
theta6[0] = -1 * atan2(B6, A6);
theta6[1] = theta6[0];
theta6[2] = theta6[3] = M_PI + theta6[0];
A6 = ny * cos(theta1[4]) - nx * sin(theta1[4]);
B6 = oy * cos(theta1[4]) - ox * sin(theta1[4]);
theta6[4] = -1 * atan2(B6, A6);
theta6[5] = theta6[4];
theta6[6] = theta6[7] = M_PI + theta6[4];
/* ----------theta5---------- */
theta5[0] = -1 * acos(ax * sin(theta1[0]) - ay * cos(theta1[0]));
theta5[1] = theta5[0];
theta5[2] = -1 * theta5[0];
theta5[3] = theta5[2];
theta5[4] = -1 * acos(ax * sin(theta1[4]) - ay * cos(theta1[4]));
theta5[5] = theta5[4];
theta5[6] = -1 * theta5[4];
theta5[7] = theta5[6];
/* ----------theta3---------- */
std::vector<double> m(8), n(8);
double record1, record2;
for (int i = 0; i <= 7; i++)
{
m[i] = this->d[4] * (sin(theta6[i]) * (nx * cos(theta1[i]) + ny * sin(theta1[i])) + cos(theta6[i]) * (ox * cos(theta1[i]) + oy * sin(theta1[i]))) - this->d[5] * (ax * cos(theta1[i]) + ay * sin(theta1[i])) + px * cos(theta1[i]) + py * sin(theta1[i]);
n[i] = pz - this->d[0] - az * this->d[5] + this->d[4] * (oz * cos(theta6[i]) + nz * sin(theta6[i]));
record1 = pow(m[i], 2) + pow(n[i], 2) - pow(this->a[1], 2) - pow(this->a[2], 2);
record2 = 2 * this->a[1] * this->a[2];
if(i % 2 == 0)
{
if(record1 / record2 <= 1.0001 && record1 / record2 > 1)
{
theta3[i] = 0;
}
else
{
theta3[i] = acos(record1 / record2);
}
}
else
{
if(record1 / record2 <= 1.0001 && record1 / record2 > 1)
{
theta3[i] = 0;
}
else
{
theta3[i] = -1 * acos(record1 / record2);
}
}
}
/* ----------theta2---------- */
std::vector<double> s2(8), c2(8);
for (int i = 0; i <= 7;i++)
{
s2[i] = ((this->a[2] * cos(theta3[i]) + this->a[1]) * n[i] - this->a[2] * sin(theta3[i]) * m[i]) / (pow(this->a[1], 2) + pow(this->a[2], 2) + 2 * this->a[1] * this->a[2] * cos(theta3[i]));
c2[i] = (m[i] + this->a[2] * sin(theta3[i]) * s2[i]) / (this->a[2] * cos(theta3[i]) + this->a[1]);
theta2[i] = atan2(s2[i], c2[i]);
}
/* ----------theta4---------- */
for (int i = 0; i <= 7;i++)
{
theta4[i] = atan2(-1 * sin(theta6[i]) * (nx * cos(theta1[i]) + ny * sin(theta1[i])) - cos(theta6[i]) * (ox * cos(theta1[i]) + oy * sin(theta1[i])), oz * cos(theta6[i]) + nz * sin(theta6[i])) - theta2[i] - theta3[i];
}
limit_angle(theta1);
limit_angle(theta2);
limit_angle(theta3);
limit_angle(theta4);
limit_angle(theta5);
limit_angle(theta6);
JointsState angles;
inverse_solutions.clear();
for (int i = 0; i <= 7; i++)
{
angles << theta1[i], theta2[i], theta3[i], theta4[i], theta5[i], theta6[i];
inverse_solutions.push_back(angles);
}
return true;
}
catch(...)
{
std::cerr << "Inverse kinematics solution failed"
<< "\n";
return false;
}
}
这一部分不必细说，就是本文最开头提到的那篇论文的复现。
需要注意的是，因为逆解很容易因为各种奇异构型而求解失败，为了不使程序崩溃报错，最好使用try catch语句。
（3）test.cpp
#include <Eigen/Core>
#include <iostream>
#include "Kinematics.h"
#include "DH_config.h"
#include "Common.h"
int main()
{
Kinematics K(UR5_DH::a, UR5_DH::alpha, UR5_DH::d);
TransMat mat;
JointsState angles;
angles << 1, 1, 1, 1, 1, 1;
K.FK(angles, mat);
std::vector<JointsState> solution;
K.IK(mat, solution);
for (int i = 0; i <= 7;i++)
{
std::cout << solution[i] << "\n"
<< "\n";
}
return 0;
}基本思路是，先用UR5的DH参数初始化一个Kinematics类的K变量，接着创建一个JointState变量来表示当前关节状态，6个关节的角度均为1弧度，然后调用K.FK方法求解当前关节状态的正解，结果存储在mat变量中，最后调用K.IK方法求解mat变量的正解。
4 CMakeList.txt
cmake_minimum_required(VERSION 3.5.0)
project(RoboticsLib)
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
file(GLOB ${PROJECT_NAME}_SOURCES "./src/*.cpp")
file(GLOB ${PROJECT_NAME}_HEADERS "./include/*.h")
include_directories(include)
include_directories(你的Eigen路径)
add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES})
add_executable(test ${${PROJECT_NAME}_SOURCES})各语句含义在3.1写一个简单的ROS通讯库中有过简单介绍。
注意：Eigen库没有库文件，只有头文件，因此只需定义好其头文件搜索路径即可。
后续进入build目录，依次执行cmake ..和make命令，即可在lib和bin下发现生成了相应文件。
标签：theta1,FT,mat,double,运动学,UR,TransMat,3.2,include 来源： https://blog.csdn.net/qq_42823342/article/details/123615699