引言

首先得到的工具座標系淨接觸力向量，然後乘以工具座標系到末端座標系的旋轉矩陣，得到末端的淨接觸力向量。然後將末端的力乘以阻尼矩陣（假設解耦），得到末端的速度向量。因為得到的速度向量還是在末端座標系下的（這裡可以考慮先轉到基座標系下再得到速度向量？），因此為了達到控制關節速度的目的，需要首先將末端的速度轉換到基座標系下，然後利用雅克比矩陣的逆，得到關節速度指令。這裡的轉換矩陣，也就是齊次變換矩陣就是這裡想要求得的。

原理

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！