引言
在前面的章节,我们分别讲了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!
评论(6)
您还未登录,请登录后发表或查看评论