概述

这个系列叫做pybullet工具坐标系位移和旋转，如题所示，这个系列讲述的是在pybullet仿真引擎中对沿机械臂工具坐标系移动和绕着机械臂工具坐标系的旋转的控制问题进行探讨。前两个章节讲讲pybullet中控制沿着机械臂的工具坐标系位移，和旋转。后面的章节将对这两种基本运动方式进行扩展，引入导纳控制等。今天我们首先来看，如何控制机械臂沿工具坐标系某一方向进行位移。

引言

注意，在这里，我们指的是沿着工具坐标系的任一方向直行。

场景设置

首先我们看热reset函数，这个函数的作用就是初始化机械臂，同时为以后机械臂绕着工具坐标系特定方向直行初始化一个方向。在这里，我们初始化了两个方向，分别是笛卡尔世界坐标系x轴方向，另一个是笛卡尔世界坐标系的任一方向。

    def reset(self):
        p.resetSimulation()
        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
        p.loadURDF("plane.urdf", basePosition=[0, 0, -0.65])
        self.UR5 = p.loadURDF("ur5_10cm_10mm.urdf", useFixedBase=True)
        p.loadURDF("table/table.urdf", basePosition=[0.5, 0, -0.65])
        # 初始化大致位置
        rest_poses = [0.019493980630576238, -1.6104386316325356, 2.371476945587111, -2.3317793248031182,
                      -1.57079440431533, 0]
        for j in range(6):
            p.resetJointState(self.UR5, j + 1, rest_poses[j])

        init_position = [0.355,0,0.22]
        init_posture = p.getQuaternionFromEuler([-math.pi / 6., 0.0, -math.pi / 8.])
        # init_posture = p.getQuaternionFromEuler([ 0.0, 0,-math.pi / 2.])
        jointposes = p.calculateInverseKinematics(self.UR5, 6, init_position,init_posture, maxNumIterations=100)
        for j in range(6):
            p.resetJointState(self.UR5, j + 1, jointposes[j])
        p.stepSimulation()
        p.enableJointForceTorqueSensor(self.UR5, 7,1)
        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)

解释下沿着世界坐标系x轴的方向是怎么设置的：

因为在前面的章节中，我们解释了pybullet逆运动学中关于姿态的意义，同时知道了机械臂轴向为y轴，因此我们只需要绕着z轴旋转-90度就可以让原先世界坐标系的y轴指向x轴方向，也就出现了图一的场景。

初始方向为x轴

如果我们想沿着工具坐标系的y轴向前移动，则可以写为：

代码如下：

def get_position_p(posture, position):  #
    matrix = np.array(pybullet.getMatrixFromQuaternion(posture)).reshape(3, 3)
    dy = 0.00050  # 沿着工具坐标系y轴向前进0.0005
    res = np.array(position).reshape(3,1)
    res += matrix[:, 1].reshape(3, 1)*dy
    return res

这里因为我们的dy是增量，因此需要和当前的位置进行相加。

当我们将运动方向该为工具坐标系x轴时：

    res += matrix[:, 0].reshape(3, 1)*dy

当设置任意方向时：

def get_position_p(posture, position):  #
    matrix = np.array(pybullet.getMatrixFromQuaternion(posture)).reshape(3, 3)
    dx = 0.0004
    dy = 0.0008
    dz = 0.0005
    res = np.array(position).reshape(3,1)
    res += matrix[:, 0].reshape(3, 1)*dx
    res += matrix[:, 1].reshape(3, 1)*dy
    res += matrix[:, 2].reshape(3, 1)*dz
    return res

初始方向为任意

其实原理大同小异，这里仅展示，沿着y轴运动的视频

May the force be with you!