引言

在机械臂与环境之间进行力交互操作时，一定会引起接触力/力矩的变化。很多情况下，我们需要根据接触力的大小和规律对机械臂的下一步运动进行规划和控制，今天我们就一起来探讨下，机械臂与环境进行力交互时，接触力/力矩的变化规律。

获取数据

获取数据使用的是python中的write函数。首先打开一个文件，然后使用参数r+，通过覆盖写的方式将信息写入指定文件中。

        with open('force.txt', 'r+') as f:
            f.write(str(self.force[0]) + ',' + str(self.force[1]) + ',' + str(self.force[2]))
            f.write('\r\n')

轴-墙数据分析

我们首先对轴和墙之间的碰撞进行实验。测试不同的轴倾角下，沿着工具轴向前进的情况下，当轴-墙接触时，接触力和力矩的变化情况。

步长选择

在步长的选择上，为了兼顾位移速度和姿态调整速度，避免出现因为纯位移速度过而使姿态调整不及时的情况。我们设置位移移动速度为0.1mm，姿态移动速度为0.85度。虽然是如此设置，但是具体应用还是要根据场景来定。因为绕着不同坐标系的旋转，也会使末端在师姐坐标系的位置变化，假设每次姿态调整以后仅位移一次，那么最终的位置将变为：

T6' = T6(Rt(0.85°，Ttool))+0.0001m*[0,1,0]

阈值选择

在阈值的选择上，分别测试了使用力矩作为阈值和使用力作为阈值时的情况，发现使用力矩作为阈值时，当轴和墙面接近平行时，力矩值会非常小。因此在这里，我们选择使用接触力来判断是否进行导纳控制。在值得选择上，我们将阈值设置为0.2。

特征分析

在特征分析部分，我们测试了不同初始偏角时的运动和受力情况，发现

1、旋转方向没有问题，但是最终的调整结果受不同旋转中心点和阈值影响比较大；

2、不同初始偏角的受力/力矩中，受力值大小区别较大；受力矩值差距不是特别大，但仍有区别。

轴-孔数据分析

前面的一系列铺垫，都是为了这个轴孔装配的场景。在实际检验过程中，仍让出现了一些bug，但也都一一化解：

2、运控函数修改

出现的第一个问题是，不管是在控制沿着轴向运动还是绕轴旋转，发现其都会出现偏差。经过分析，发现虽然之前使用stepstimulation 100次的方式进行了改进，但是对于高精度任务仍然不适用，因此在这里，我们对控制函数再次进行改进。

如前面文章介绍的，我们在控制函数针对不同的运动方式设置了不同的控制函数。比如单独设置的旋转函数，单独设置的位移函数。而这些偏差发生的原因就隐藏在这些控制函数中。通过偏差的不断累积，会使得最终的位置出现很大的偏差。我们的解决思路主要分为两块：

首先在单独设置的函数中，只设置与此函数目的相同的变量，而另一个变量则不进行修改。例如在位置控制的时候，我只需要设置期望位置信息，期望姿态信息不需要进行额外的设置，继续沿用姿态控制结束或者初始目标姿态就可以。：

然后，在之前计算目标位置或姿态时，我们都是基于当前的的状态量，在当前的基础上进行控制。由于当前的状态量（位置，姿态）是存在偏差的，在此基础山再次更新会使使误差不断累积。因此在单独控制函数的输入部分，我们采用上一步计算的期望值，而不是经过控制之后得到的采样值进行计算，以姿态导纳控制为例：

原来的函数为：

现在：

2、模型设置

在模型设置部分，遇到了一个大bug：在导入模型的时候，我们首先将soliderworks绘制的模型转为urdf，然后在控制文件中设置导入此urdf文件。但是在导纳控制测试时却遇到了问题，在控制其运动时，经常会调整过头；或者出现向相反方向运动的情况。下图很好地说明了这个问题：

可以看到，尽管轴还未碰到工件孔，但是好像被什么神秘力量控制住了。

回归正题，总之就是因为soliderworks转urdf后的模型在pubullet中碰撞检测部分并不会按照模型原本的形状进行碰撞检测。为此，我在urdf的碰撞属性部分摒弃了使用stl文件的做法，而是直接使用圆柱模型作为碰撞模型。

如有知道原因的朋友，请在评论区或者私信联系我，不尽感激！

轴如此设置之后，孔模型按理说也需要这样设计，但是并没有这样现成的模型。那么孔模型的碰撞属性也只能先用stl文件了，只是需要在碰撞标签上加上：

设置完成后，再测试模型：

May the force be with you!