引言

在前面的设置中，我们分别讲述了UR5机械臂的DH参数、模型，以及测试环境的设置。这一节我们将从UR5机械臂的运动，包括位移和旋转；以及力传感器信息值经过旋转变换后的表示两方面进行讲述，首先我们先来看机械臂的运动。

机械臂运动

在机械臂运动方面，如上一节所示，我们已经在开始运动前的reset函数中设置了机械臂处于一个大致正确的位姿，是通过指定关节角的方式设置的。那么如果我们想要设置机械臂末端的笛卡尔空间位姿为我们指定的位姿时，就必须用到笛卡尔空间的位姿控制。这就用到了机械臂的逆运动学，虽然在前面的章节中，我们也介绍过pybullet中用于机械臂逆运动学的接口函数，并对解算过程中的迭代次数进行对比实验，但是并没有关注这个函数前面的几个关键参数，那我们首先再来看一下这个函数的关键参数，也就是位置和姿态的设置方式把。我们先来看一下pybullet手册中是怎么记载的：

可以看到在参数中首先是模型编号，这个在我们的实验中为self.UR5；然后是末端连杆的编号，这里说是末端连杆，其实哪个连杆都可以，在这里我们一般设置末端连杆为6连杆。也就是对应最后一个连杆坐标系。接下俩就是主要的参数了，首先看targetPosition，这里的这个位置，指的是末端连杆的位置在笛卡尔世界坐标系中的表示；然后我们看targetOrientation，他指的是当前连杆坐标系在笛卡尔世界坐标系的目标姿态。所以我们知道，不管是位置还是姿态，都是相对于世界坐标系的。

那么在这里,我们主要有两种设置方法:

1、末端坐标系姿态已知

末端坐标系已知这件事可以从DH参数或者从图中看出等方式得到，在第一节中，我们发现pybullet中的连杆坐标系建立方式不同于标准DH参数法建立的坐标系，因此我们通过debugline得到了末端连杆轴向为其坐标系的y轴，再通过找到其z轴轴向后，我们其实已经可以得到末端坐标系相对于笛卡尔世界坐标系的旋转了。

self.posture = p.getQuaternionFromEuler([ -math.pi/2, 0, -math.pi/2])

这里需要注意的是，尽管当前的末端连杆坐标系已经绕着笛卡尔世界坐标系进行了如上欧拉角角度的旋转，但是由于这里我们需要给出的是在笛卡尔世界坐标系中的姿态，并不是增量式的改变，是绝对的姿态表示，因此这里仍需要设置此欧拉角，并不能设置[0,0,0]，而且由于初始化是通过关节角初始化，并不能保证姿态绝对正确，因此也必须在这里将option的姿态项设置。

由于旋转完全是和初始姿态相同，因此这里的位置项就可以直接设置为当前位置：

self.position = p.getLinkState(self.UR5,6)[4]

如下为初始位姿和最终位姿：

2、仅知道轴向，并不知全部姿态

这个时候，由于目标姿态在世界坐标系中的姿态并不知道，也也就是说我们有好多种可能的目标姿态。这时，我们往往会选择最简单的一种，例如：

self.posture = p.getQuaternionFromEuler([ -math.pi/2, 0, 0])

这也没有问题，主要的问题是在位置设置上。由于目标姿态可能不是当前的姿态，因此机械臂在运动时，还需要绕着已知轴（这里时y轴）进行旋转，来调整姿态到达我们设定的目标姿态。而在调整的过程中，就会导致位置项发生偏移，我们继续执行，发现：

而当我们将目标位置设置为固定的值，并且执行两边逆运动学的时候，发现：

结果正常了。代码如下：

self.position = [0.3596462309360504, 0.11618257313966751, 0.24329537153244019]
print("初始位姿：",'\n',p.getLinkState(self.UR5,6)[4],'\n',p.getLinkState(self.UR5,6)[5])
self.posture = p.getQuaternionFromEuler([ -math.pi/2, 0, 0])
self.go()#一遍
self.go()#两遍
print("最终位姿：",'\n',p.getLinkState(self.UR5,6)[4],'\n',p.getLinkState(self.UR5,6)[5])

传感器值表示

在获取传感器值得时候，为我们需要先看下机械臂得urdf文件描述：

我们看到，在工具坐标系的设置中，此固定关节在上一关节的基础上，绕着其z轴旋转了90度，这体现在接触力上，就是

我们在得到工具坐标系的接触力后，再经过旋转得到末端连杆坐标系下的表示：

self.rotate = np.array(p.getMatrixFromQuaternion(p.getQuaternionFromEuler([0., 0.0, math.pi / 2.])),dtype=float).reshape(3, 3)
force = np.array(p.getJointState(self.UR5, 7)[2][0:3],dtype=float).reshape(3,1)
self.force = np.dot(self.rotate,force)

最终对比六轴受力和七轴等效到六轴受力：

两者一毛一样了

最后还需要注意，pybullet会将连杆的旋转也看作关节旋转，因此在这里我将第六关节的旋转去掉：

                                                  |

                                                  |

                                                  |

May the force be with you！