本文隶属于工程机械臂末端柔顺控制(Ros+Gazebo仿真实现)
0 前言
之前写了一个UR5的运动学库,参考3.2 写一个UR机器人运动学库,发现该运动学库需要补充一部分内容,主要是关于雅各比矩阵的,后面能不能用到雅各比不知道,但是运动学库里面怎么能不包含雅各比矩阵呢?所以先写上去再说。
于是乎添加了一个获取当前雅各比矩阵的函数,代码如下:
bool Kinematics::GetJacobian(const JointsDisplacement &angles, JacobianMat &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);
TransMat T13 = T12 * T23;
TransMat T14 = T13 * T34;
TransMat T15 = T14 * T45;
TransMat T16 = T15 * T56;
TransMat T17 = T16 * T67;
Direction o1, o2, o3, o4, o5, o6, o7;
o1 << 0.0, 0.0, 0.0;
o2 << T12(0, 3), T12(1, 3), T12(2, 3);
o3 << T13(0, 3), T13(1, 3), T13(2, 3);
o4 << T14(0, 3), T14(1, 3), T14(2, 3);
o5 << T15(0, 3), T15(1, 3), T15(2, 3);
o6 << T16(0, 3), T16(1, 3), T16(2, 3);
o7 << T17(0, 3), T17(1, 3), T17(2, 3);
Direction z1, z2, z3, z4, z5, z6, z7;
z1 << 0.0, 0.0, 1.0;
z2 << T12(0, 2), T12(1, 2), T12(2, 2);
z3 << T13(0, 2), T13(1, 2), T13(2, 2);
z4 << T14(0, 2), T14(1, 2), T14(2, 2);
z5 << T15(0, 2), T15(1, 2), T15(2, 2);
z6 << T16(0, 2), T16(1, 2), T16(2, 2);
z7 << T17(0, 2), T17(1, 2), T17(2, 2);
auto Jv1 = z1.cross(o7 - o1);
auto Jv2 = z2.cross(o7 - o2);
auto Jv3 = z3.cross(o7 - o3);
auto Jv4 = z4.cross(o7 - o4);
auto Jv5 = z5.cross(o7 - o5);
auto Jv6 = z6.cross(o7 - o6);
mat << Jv1(0), Jv2(0), Jv3(0), Jv4(0), Jv5(0), Jv6(0),
Jv1(1), Jv2(1), Jv3(1), Jv4(1), Jv5(1), Jv6(1),
Jv1(2), Jv2(2), Jv3(2), Jv4(2), Jv5(2), Jv6(2),
z1(0), z2(0), z3(0), z4(0), z5(0), z6(0),
z1(1), z2(1