引言

在前面的章节中，我们讲解了柔顺控制原理，python微分运动求解，为后面求解位置导纳控制打下基础。今天我们来讲解下如何使用python在pybullet仿真环境中实现姿态导纳控制。

环境设置

在位置导纳控制那一章中，我们使用盆作为墙面，来测试轴碰撞前面后的导纳控制效果。在实际的控制中，我们首先在reset中，使用设置的M,B,K参数获得需要的微分方程，进而求得带外力符号的特解（自变量设置为固定值0.005，这是pybullet一步的时间），然后在主程序循环中，赋值当前受力和当前位置，输出期望的位置，其中姿态设置为定值。

在位置导纳控制那一节中，我们设置盆作为墙面，进行位置导纳控制。在这一节，我们仍然使用哪个墙面进行实验，只不过这次我们需要实现的是：撞墙之后可以实现姿态调整进行实现柔顺。

代码逻辑分析

1、首先，我们需要在reset中实现初始化位姿的设置，以及导纳模型的搭建以及解的求取：

2、然后我们回到主函数中，在主函数中，我们需要实现当前状态获取以及计算期望位姿：

3、首先，我们看更新当前状态的函数：

在这个函数中，我们首先实现了当前位姿的更新。在得到当前位姿的同时，我们还需要获取工具坐标系中所受力矩。由于需要对工具坐标系力矩转移到机器人末端连杆坐标系，我们需要将力矩保存格式转为ndarray。在得到机器人末端连杆坐标系的距离之后，为了能够使力矩数据格式适配计算所需转动轴角（姿态）的函数。需要将其转为list格式，并进行降维。

4、执行完更新当前状态的函数之后，就可以利用所获得的数据进行控制。

在这里，我们有两个控制函数，分别是仅直行的控制函数，一个是仅姿态导纳控制的控制函数，两者之间通过力矩阈值进行切换。

在仅直行控制函数中：

    def update_control_p_with_adr(self):
        self.position_need = get_position_p(self.posture, self.position,2)
        self.posture_need = p.getQuaternionFromEuler([-math.pi / 2., 0, -math.pi / 2.])
        #在这里设置可以保证导纳控制时位置不变，因为导纳控制时位置已经fixed
        self.fixed_position = self.position_need

期望位置同之前章节中介绍直行一样，我们沿着z轴，每次运动0.0002m；期望姿态每次保持固定不变。这里需要注意的是：这里我们设置一个单独的变量self.fixed_position，用于在导纳控制中位置量self.position_need的赋值。

在导纳控制的函数中：

在这个控制函数中，同样的需要为期望位置和期望姿态赋值。期望位置赋值就是刚才在直行中设置的量。因为我们总是需要先进行位置控制，这可以保证self.fixed_position的值是有意义的。在姿态控制时，我们首先得到导纳控制的解，然后归一化得到轴角形式的值，最后最后转为四元数，得到期望姿态。最终的结果分析部分见下文。

May the force be with you!