引言

在前面的章节中，我们分别介绍了机械臂沿着末端任意方向直行和旋转，以及绕着工具坐标系任意旋转坐标原点旋转，今天我们将之前这些功能综合以下，为后面实现姿态导纳功能能打坚实基础。

场景介绍

在本节中，我们需要实现的是直行和旋转的综合控制，为了测试起见，我们以直行-旋转-直行的策略进行设置。

在前面的介绍中，我们讲到：

1、当进行直行的位移控制时，首先传输当前姿态，以及当前位置，函数将输出下一步的连杆坐标系原点在世界坐标系中的位置。

在实际控制时，位置是下一步的连杆坐标系原点在世界坐标系中的位置；姿态保持不变；

2、当进行旋转控制时，我们传输当前姿态以及待旋转的姿态（欧拉角），函数将输出下一步的姿态。

在实际控制时，位置为刚开始旋转时的位置，保持不变；姿态为函数输出下一步连杆坐标系在世界坐标系中的姿态。

代码

使用成员变量控制运动模式，并在直行的时候，消除旋转时特定的固定位置，以便下次旋转时能够重新记忆。

    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)
    def update_control_r(self, e):
        self.posture_need = get_position_r(self.posture,e)
        if any(self.fixed_position):
            self.position_need = self.fixed_position
        else:
            self.fixed_position = self.position
            self.position_need = self.fixed_position
    def update_control_p(self):
        self.position_need = get_position_p(self.posture, self.position)
        self.posture_need = self.posture
        self.fixed_position=[0]
    def revolove(self,e):
        self.update_state()
        self.update_control_r(e)
        self.go()
    def straight(self):
        self.update_state()
        self.update_control_p()
        self.go()
    def step(self):
        self.record = self.record-1
        if self.record>200:
            self.straight()
        elif self.record>80:
            self.revolove([0,0,math.pi/200.])
        else:
            self.straight()

效果展示

进阶

完成以上工作之后，我们将旋转变为变旋转中心，变旋转中心和普通旋转在代码上的区别就是关于位置的设定，普通旋转时，位置保持不变；在变旋转中心时，位置需要实时变化，而为了程序执行的正确性，这时的当前位置采用上一次计算的位置，而不是实时采集。

两者区别如下：

#普通   
 def update_control_r(self, e):
        self.posture_need = get_position_r(self.posture,e)
        if any(self.fixed_position):
            self.position_need = self.fixed_position
        else:
            self.fixed_position = self.position
            self.position_need = self.fixed_position
#变
    def update_control_r_variety(self, e):
        if any(self.fixed_position):
            self.position = self.fixed_position
        else:
            self.position = self.position
        self.fixed_position,self.posture_need = get_position_r_variety(self.position,self.posture,e,0.05)
        self.position_need = self.fixed_position

效果如下：

May the force be with you！