笛卡尔空间力反馈的关节速度控制之机器人运动学库KDL安装及使用

引言

首先得到的工具坐标系净接触力向量，然后乘以工具坐标系到末端坐标系的旋转矩阵，得到末端的净接触力向量。然后将末端的力乘以阻尼矩阵（假设解耦），得到末端的速度向量。因为得到的速度向量还是在末端坐标系下的（这里可以考虑先转到基坐标系下再得到速度向量？），因此为了达到控制关节速度的目的，需要首先将末端的速度转换到基坐标系下，然后利用雅克比矩阵的逆，得到关节速度指令。这里的转换矩阵，也就是齐次变换矩阵就是这里想要求得的。

原理

1、雅克比矩阵可以通过moveit中的命令获取，这里不知道为什么ur自带的指令总是和matlab（目前已知应该是正确）有点出入，在正负号方面。因此首先进行相关符号的转换（不会是坑吧，算了，先挖再说。）
2、通过辨识的旋转信息，将工具坐标系下的力转为末端坐标系;然后使用阻尼矩阵（相当于阻尼控制，将末端力转为末端速度），得到在末端坐标系下的速度信息。再经过旋转矩阵和雅克避矩阵的转化，从而得到基坐标系下的笛卡尔空间速度值。如引言部分所示

安装

由于雅克比矩阵可以通过moveit获得，但是将末端坐标系下的速度转为基坐标系下的工作需要用到机械臂的正向运动学。一种方法是直接通过手算正向运动学，另一种方法是通过linux下的KDL库
（机器人运动学相关）直接获取。这里我们选择使用KDL进行正运动学求解。因此关于在linux下正确安装使用KDL库，是本文所介绍的。
安装主要是参见这篇文章链接。
这里我们选择用py版本的KDL