引言
在前面两节中,我们分别介绍了柔顺控制的原理以及如何求解微分方程。经过柔顺控制章节的讲解,大家也能猜到我们要做的实验就是导纳控制。对,我们今天来看一下如何在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!
评论(3)
您还未登录,请登录后发表或查看评论