引言

机械臂的姿态变换，在这一节，主要讲述的是，机械臂绕着工具坐标系进行旋转，以及旋转特定角度的问题。

场景介绍

在机械臂旋转的这个任务中，和沿着某一特定方向运动类似，都需要初始化机械臂的位置和姿态，这里我们首先初始化机械臂的姿态为：轴向为笛卡尔世界坐标系的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()
        self.position_need = p.getLinkState(self.UR5,6)[4]
        p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)

其中，self.position_need指的就是待运动的位置。在以后的设置中，仅需要获取新的姿态信息就可以。

绕x轴转

我们以绕x轴旋转为例。对于绕着末端坐标系的旋转来说，旋转矩阵相乘的作用结果就是绕着最后一个坐标系的旋转，这是机器人正向运动学所规定的。

在这里我们首先将待旋转的欧拉角转为四元数，然后利用四元数的乘法（作用等同于旋转矩阵）。获取旋转后末端坐标系在笛卡尔世界坐标系的表示，而位置则为reset中获取的信息，保持不变，代码如下：

def get_position_r(posture,E):
    a = [key for i in posture for key in i]
    need = np.array(pybullet.getQuaternionFromEuler(E)).reshape(4,1).tolist()
    b = [key for j in need for key in j]
    pos = np.array(quaternion_rotation(a,b)).reshape(4,1)
    return pos

得到的动态图如下：

绕特定轴转

需要注意的是，欧拉角表示时的步长，在这里选择的是：

[math.pi/2000.,0,math.pi/3000.]

代码

#更新由旋转引起运动
    def update_control_r(self, e):
        self.posture_need = get_position_r(self.posture,e)
# 更新姿态
    def update_state(self):
        self.posture = np.array(p.getLinkState(self.UR5, 6)[5],dtype=float).reshape(4,1)
#旋转
    def revolove(self,e):
        self.update_control_r(e)
        self.go()
#运动
    def step(self):
        p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)  #平滑视觉渲
        self.update_state()
        self.revolove([math.pi/2000.,0,math.pi/3000.])

May the force be with you!