引言

在前面的章节,我们分别讲了UR5机械臂的DH参数及其坐标系;测试环境的设置;机械臂运动以及接触力表示。今天这一节,我们将讲述在设置过程中的代码规范,并给出整体代码。

go()

在前面讲机械臂的运动时,代码中曾经出现了一个自定义函数,我把它取名为go(),这个函数实现的功能也如其名,就是去执行运动的功能。那么我们先来看它的代码:

首先映入眼帘的就是逆运动学接口了,和前面讲逆运动学部分相同,这里需要设置最大迭代结算次数和持续仿真时间。这里的位置和姿态,我用了成员变量的方式,在step或者reset中设置值后,然后执行无参的go()就可以。那么我们可能想问,如果我们想要控制的坐标系并不仅是第六连杆怎么办?好问题,那这样的话,就必然涉及到传参,我们设置不仅连杆编号可以变,同时位置和姿态也由传参得到。所以我们将代码修改为如下:

这种设置方式会显得更加灵活。

set_place()

因为我们的测试场景中有两堵墙,而每次只能测试一面的效果,为了,我们可以使用函数指针的方式,重新编排代码结构:

       self.state = {
            'front': self.front,
            'left': self.left
        }
    def front(self):
        orientation_wall1 = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
        p.loadURDF("tray/tray.urdf", basePosition=[0.8, 0, 0.25], baseOrientation=orientation_wall1)
        position = p.getLinkState(self.UR5,6)[4]
        posture_origin = p.getLinkState(self.UR5, 6)[5]
        up_to_wall1 = p.getQuaternionFromEuler([math.pi / 2., 0.0, 0.0])
        posture = self.quaternion_rotation(posture_origin, up_to_wall1)
        self.go(6,position,posture)
        posture = np.array(p.getLinkState(self.UR5, 6)[5], dtype=float).reshape(4, 1)
        m = 1000000
        while (m):
            m = m - 1
            self.step(0,posture)
    def left(self):
        orientation_wall2 = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi])
        p.loadURDF("tray/tray.urdf", basePosition=[0, 0.9, 0.25], baseOrientation=orientation_wall2)
        position = p.getLinkState(self.UR5,6)[4]
        posture_origin = p.getLinkState(self.UR5, 6)[5]
        up_to_wall2 = p.getQuaternionFromEuler([0.0, 0.0,math.pi / 2.])
        posture = self.quaternion_rotation(posture_origin, up_to_wall2)
        self.go(6,position,posture)
        posture = np.array(p.getLinkState(self.UR5, 6)[5], dtype=float).reshape(4, 1)
        m = 1000000
        while (m):
            m = m - 1
            self.step(1,posture)
    def move(self,name):
        self.state[name]()

当我们在左转直行的时候,却发现,机械臂不动了。原因是机器人的构型出现了问题,因此在此基础上,我们在go之后手动转动关节,得到:

        position = p.getLinkState(self.UR5,6)[4]
        posture_origin = p.getLinkState(self.UR5, 6)[5]
        up_to_wall2 = p.getQuaternionFromEuler([math.pi / 2.,0.0, 0.0])
        posture = self.quaternion_rotation(posture_origin, up_to_wall2)
        self.go(6,position,posture)
        self.go(6,position,posture)
        p.setJointMotorControl2(self.UR5,1,p.POSITION_CONTROL,math.pi/2,positionGain = 10.0)
        p.stepSimulation()
        p.stepSimulation()
        p.stepSimulation()
        p.stepSimulation()
        p.stepSimulation()
        p.stepSimulation()
        p.stepSimulation()

得到分别左转和前进的仿真图:

结果

向左:

向右:

因此,可以发现这个受力值是在末端坐标系下的受力值。

如果想转到笛卡尔世界坐标系,只需要再乘机械臂本身的正运动学矩阵就可以。那这个系列就到此为止了。

关注我,带你学习机器人!

May the force be with you!