引言

在前面两节中,我们分别介绍了柔顺控制的原理以及如何求解微分方程。经过柔顺控制章节的讲解,大家也能猜到我们要做的实验就是导纳控制。对,我们今天来看一下如何在pybullet中进行位置导纳控制。

原理

在进行实验之前,我们已经可以通过之前柔顺控制章节的讲解,了解了导纳控制的原理。本次实验就是基于导纳控制原理进行的操作。我们还是使用盆作为墙面,然后控制机械臂直行,让轴撞击墙。在得到初始规划的位置,并进行控制之前,我们通过导纳控制算法,在接触力的参与下,利用微分方程求解函数,得到位置偏差。将其加到初始规划的位置上之后,将得到修正后的位置量。在这里,由于接触力中不止一个方向,因此为了使效果更加明显,我们对姿态信息进行了固定。

代码实践

我们在reset中得到在使用柔顺控制模型情况下的求解结果:

    def reset(self):
        self.admittance(1,1.414,1)
        p.resetSimulation()
        ...

在这个导纳控制函数中,我们将返回:

        self.res = r1.subs(a[0]).subs({M:m_value,B:b_value,K:k_value,t:0.005}).rhs

将其作为类成员变量,以供其他函数调用。其中t=0.005,是根据stepstimulation的时间为1/240决定的。

得到模型以后,就可以在setp中,通过给接触力和当前位置赋值的方式,得到修正后期望的位置。其中setp代码如下:

    def step(self):
        p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)  #平滑视觉渲染
        self.update_state()
        self.ad()

在状态更新函数中,主要是更新接触力,在这注意将接触力反向,且由于位置量为在世界坐标系下的表示,因此需要将接触力也转移到世界坐标系下。

    def update_state(self):
        self.posture = np.array(p.getLinkState(self.UR5, 6)[5],dtype=float).reshape(4,1)
        self.position = np.array(p.getLinkState(self.UR5, 6)[4],dtype=float).reshape(3,1)
        force = np.array(p.getJointState(self.UR5, 7)[2][0:3],dtype=float).reshape(3,1)
        force1 = [-j for i in np.dot(self.rotate,force).tolist() for j in i]
        matrix = np.array(p.getMatrixFromQuaternion(self.posture)).reshape(3,3)
        self.force_com = np.dot(matrix,force1)

在self.ad()函数中:

    def ad(self):
        self.update_control_p()
        self.position_com = [j for i in self.position_need for j in i]
        self.compute(self.position_com,self.force_com)
        self.go()

其中更新新的位置量的函数为:

    def update_control_p(self):
        self.position_need = get_position_p(self.posture, self.position,2)
        self.posture_need = p.getQuaternionFromEuler([-math.pi / 2., 0, -math.pi / 2.])

也就是说,新的位置还是由规划产生,姿态不变,得到新的位置之后,先在self.ad()中,将其转为一维列表,即为初始规划位置。并将其和接触力一起放入导纳控制函数中,得到新的调整后的位置:

        self.admittance_position[0] = complex(self.res.subs({ fext:f[0]}).evalf()).real+x0[0]
        self.admittance_position[1] = complex(self.res.subs({ fext:f[1]}).evalf()).real+x0[1]
        self.admittance_position[2] = complex(self.res.subs({ fext:f[2]}).evalf()).real+x0[2]
        self.position_need = self.admittance_position

最后再直行go()开始运动

对比

我们设置Δx = 0.0002,M,K,B的值分别是:1,1.141,1时

接触力:

当不使用柔顺控制时:

May the force be with you!