系列概述

在上一节博客中，我们讲述了如何在pybullet中采集末端六维力，即力和力矩。今天我们就针对pybullet中UR接触力的设置进行讲解。首先需要构建UR5机械臂的坐标系模型；然后对接触力测试环境进行分析，得到关节处的接触力值；那么如何将接触力转换到基坐标系，或者末端坐标系下进行表示，是这个系列第三篇博客主要介绍的内容；这个系列的第四篇博客将针对整个项目的代码规范化进行讲解，主要用到了函数指针的概念。那么现在就让我们进入本节的内容，UR5坐标系构建吧。

引言

UR5机械臂是一款被广泛应用于工业和学术界的机械臂。它由六自由度的连杆构成，接下来我们来看一下UR5机械臂的DH参数、坐标系建立、在pybullet中的坐标吧。

DH参数

DH参数是描述每个连杆坐标系相对与上一连杆坐标系的平移和旋转参数，进而我们可以通过正向运动学得到机器人的正向运动矩阵，也就是齐次变换矩阵。DH参数法建立坐标系可以分为标准法和修正法两种。在这里，我们采用经典法建立坐标系的方式建立连杆坐标系。最终得到UR5机械臂的DH参数表如下所示：

我们可以得到最终的机械臂连杆坐标系简图如下：

图均出自我的论文Geometric model-based joint angle selection criterion for force parameter identification & Decoupling control method of position and posture in shaft-hole assembly DOI:10.1109/CYBER53097.2021.9588304

坐标系建立

由上面的DH参数，我们可以得到坐标系简图，但是我们需要注意的是，在rviz中，坐标系的建立并不是按照标准dh参数法建立的，这也就导致，最终的坐标系图与之前看到的不一样。我们再在rviz中检验，即首先运行ur5的launch文件，然后运行rviz显示机械臂，最后勾选显示坐标系：

可以看到，在rviz中，机械臂的末端坐标系轴向为y轴。而不是标准法中的z轴。

pybullet

最后，我们回到pybullet中检验坐标系的建立，在pybullet中，没有直接显示坐标系的函数接口，我们可以通过使用addUserDebugLine这个函数检验。关于这个函数的介绍如下：

可以看到，我们需要传入的参数为在世界坐标系下的坐标，但是我们希望的是，通过在末端坐标系的y轴和z轴分别划线，来判断末端轴向为y轴还是z轴，从而判断DH参数带来的坐标系建立是否为使用标准方法建立的。因此我们有如下设计：

1、判断末端连杆坐标系的y轴

这里使用旋转将末端坐标系下，作为debugline中末端点的y轴的1m转到世界坐标系中。

        start = list(p.getLinkState(self.UR5,6)[4])
        rot = np.array(p.getMatrixFromQuaternion(p.getLinkState(self.UR5,6)[5])).reshape(3,3)
        add_local = np.array([0,1,0]).reshape(3,1)
        add_world = np.dot(rot,add_local).tolist()
        add_world_one = [one for token in add_world for one in token]
        end = [a+b for a,b in zip(start,add_world_one)
        p.addUserDebugLine(start,end,[0,0,0])

1、判断末端连杆坐标系的z轴

同理，只需要将[0,1,0]转为[0,0,1]就可以

        add_local = np.array([0,0,1]).reshape(3,1)

那么我们就可以看到，果然确实不是按照标准DH方法建立的，虽然如此，但是我们知道了末端坐标系中，y轴方向与末端关节轴向方向相同，这对后面坐标变换也起到至关重要的作用。

参考：

https://ieeexplore.ieee.org/document/9588304

May the force be with you!