带有onrobot_rg2夹爪的ur3机械臂在pybullet中的仿真

Simulation of ur3 with onrobot_rg2 by pybullet

本文对带有onrobot_rg2的ur3机械臂在pybullet虚拟世界中进行仿真，能够控制夹爪的张合，机械臂的各关节角度，具体效果如动图所示.

仿真是在参考Chen matafela的 pybullet_grasp_annotator_robotiq_85.基础上完成的，onrobot_rg2夹爪的urdf模型是我用SW的插件导出的，ur3机械臂的urdf模型可以用xacro文件转换.感谢我的师兄帮我找到了这个资料，让我能够有了进一步的进展.话不多说，直接进入到代码的讲解.

import pybullet as p
import pybullet_data as pd
// connect to engine servers
physicsClient = p.connect(p.GUI)
// add search path for loadURDF
p.setAdditionalSearchPath(pd.getDataPath())

p.connect(p.GUI)用来启动仿真，setAdditionalSearchPath()用来寻找目录中的文件.
接下来，定义虚拟环境中的世界，添加下面这句代码.

// define world
p.setGravity(0,0,-9.8)
p.resetDebugVisualizerCamera(cameraDistance=2,cameraYaw=0,cameraPitch=-40,cameraTargetPosition=[0.5,-0.9,0.5])

p.setGravity(0,0,-9.8)这句代码在虚拟世界中添加重力，调整视窗用p.resetDebugVisualizerCamera ()，参数中cameraTargetPosition指摄像机的位置，cameraDistance指相机的焦距的变化，类似变焦.
然后，就是导入机器人模型，robotStartPos和robotStartOrn表示的是机器人起始时的位姿.

robotUrdfPath = "./ur3/ur3_visual_grabbing_with_rg2.urdf"
robotStartPos = [0, 0, 0]
robotStartOrn = p.getQuaternionFromEuler([0, 0, 0])
robotID = p.loadURDF(robotUrdfPath, robotStartPos, robotStartOrn,
                     flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)

在虚拟世界中添加一些物体，包括桌子、kinect_v2相机和小方块，用于接下来的视觉抓取.

//load Object
tableUid = p.loadURDF("table/table.urdf",basePosition=[0.5,0,-0.65])
kinect_v2Uid = p.loadURDF("ur3/kinect_v2.urdf",basePosition=[0.4,0,1.57])
objectUid = p.loadURDF("cube_small.urdf",basePosition=[0.7,0,0])

from collections import namedtuple
from attrdict import AttrDict

导入两个python模块，namedtuple和AttrDict.namedtuple是继承自tuple的子类.namedtuple创建一个和tuple类似的对象，而且对象拥有可访问的属性.下面代码定义了一个namedtuple类型的jointInfo，并包含id、name、type、lowerLimit、upperLimit、maxForce和maxVelocity属性.
python的Attrdict模块能够像给类对象赋值一样修改dict对应key的value，下面代码创建了一个空的字典joints用来存储各关节的信息.
p.getNumJoints(robotID)用来得到各关节的序号值.

jointTypeList = ["REVOLUTE", "PRISMATIC", "SPHERICAL", "PLANAR", "FIXED"]
numJoints = p.getNumJoints(robotID)
jointInfo = namedtuple("jointInfo",
                       ["id","name","type","lowerLimit","upperLimit","maxForce","maxVelocity"])

joints = AttrDict()
//  get jointInfo
for i in range(numJoints):
    info = p.getJointInfo(robotID, i)
    jointID = info[0]
    jointName = info[1].decode("utf-8")
    jointType = jointTypeList[info[2]]
    jointLowerLimit = info[8]
    jointUpperLimit = info[9]
    jointMaxForce