机器人运动学库KDL安装及使用
引言
原理
安装
PyKDL
pykdl_utils
使用
实验

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

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

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

PyKDL
首先安装PyKDL。
安装过程中会遇到问题，我是通过下载低版本的包解决的。

pykdl_utils
在文中提到了将.so文件拷贝可以解决存在的问题，但是试了以后发现不行，最终的办法是必须通过添加路径的方法。就是在下面“使用”一节的链接中提到的。
类似于这两种

sys.path.insert(0, "/home/file/catkin_ws/src/Basic_math/hrl-kdl-indigo-devel/hrl_geom/src")
sys.path.insert(0, "/home/wangxu/catkin_ws/src/Basic_math/hrl-kdl-indigo-devel/pykdl_utils/src")
但是必须要到build文件夹，就像：
```python
sys.path.insert(0, "/home/xsm/pydk/src/hrl-kdl-indigo-devel (2)/hrl_geom/src")
sys.path.insert(0, "/home/xsm/orocos_kinematics_dynamics-1.3.1/python_orocos_kdl/build")

使用
参见博客链接。当然这里也是有问题，就是对于变换矩阵的顺序，可能需要再调整一下。
当遇到存在解释解释包，但是不在此解释器下的情况时，可以在pycharm中通过为解释器添加包的方式添加。

实验
最终初步实现了末端力反馈的关节速度控制，视频以后放。

希望我们可以不忘初心，May the force be with you！