步骤详解：
1.获取状态：关节的位置和速度、末端的位置和姿态、雅可比矩阵
2.对目标空间的配置、刚度、位姿、期望外力参数进行一阶低通滤波
updateFilteredNullspaceConfig()
updateFilteredStiffness()
updateFilteredPose()
updateFilteredWrench()
3.计算空间误差（6维：位置误差+姿态误差）
4.雅可比矩阵伪逆(根据末端速度求关节速度/已知目标位置求各个关节怎么动)
两者关系式为：
为什么求伪逆？非方阵、奇异、冗余
https://github.com/matthias-mayr/Cartesian-Impedance-Controller/blob/master/src/cartesian_impedance_controller.cpp#L16