在pybullet中使用urx控制真实的ur3机械臂和onrobot_rg2夹爪

1.运行环境

• PyCharm
• python3.6
• urx
• pybullet

2.环境的搭建

urx的介绍和克隆地址在这里，使用这里的urx会在运行过程中报错，例如：No handlers could be found for logger "ursecmon"，网上找到的回答是：That warning is not a bug, just some bad default configuration in very old python versions，是因为python版本问题导致的正常bug。还有一个致命的问题是，它只支持robotiq夹爪的控制。幸好网上有大神已经完美解决了这两个问题，分别是jkur和Mofeywalker，前一个解决了在运行过程中报错的问题，后一个解决了onrobot_rg2夹爪的控制。我对两位大神的代码进行了整合，放在了我的github里，需要的可以在这里🔗克隆，克隆之后放在/home/user/.local/lib/python3.6/site-packages这个文件夹，user是你的用户名。
接下来安装pybullet，在终端运行

$ pip3 install pybullet

最后，在PyCharm中敲代码，环境选择python3.6，我写了一个ur3robot的API，用来实现在pybullet中使用urx控制真实的ur3机械臂和onrobot_rg2夹爪。
ur3robot的完整代码如下，运行完之后会提示缺少模块，缺啥补啥。

import pybullet as p
import numpy as np
import math
import time
import urx
from collections import namedtuple
from attrdict import AttrDict
from urx.gripper import OnRobotGripperRG2

useNullSpace = 1
ikSolver = 0
ur3NumDofs = 6

#lower limits for null space
ll = [-7]*ur3NumDofs
#upper limits for null space
ul = [7]*ur3NumDofs
#joint ranges for null space
jr = [7]*ur3NumDofs
#restposes for null space
rp = [0, -0.5 * math.pi, 0, 0, 0.5 * math.pi, 0]

gripper_main_control_joint_name = "gripper_finger1_joint"
EndEffector_joint_name = "ur_grasptarget_hand"
mimic_joint_name = ["gripper_finger2_joint",
                    "gripper_finger1_inner_knuckle_joint",
                    "gripper_finger2_inner_knuckle_joint",
                    "gripper_finger1_finger_tip_joint",
                    "gripper_finger2_finger_tip_joint"]
mimic_multiplier = [-1, 1, -1, -1, 1]
mimic_offset = [0, 0, 0, 0, 0]
position_control_joint_name = ["shoulder_pan_joint",
                               "shoulder_lift_joint",
                               "elbow_joint",
                               "wrist_1_joint",
                               "wrist_2_joint",
                               "wrist_3_joint"]

class ur3Sim(object):
    def __init__(self, bullet_client, offset, ip):
        self.rob = urx.Robot(ip)
        self.bullet_client = bullet_client
        self.bullet_client.setPhysicsEngineParameter(solverResidualThreshold=0)
        self.offset = np.array(offset)

        flags = self.bullet_client.URDF_ENABLE_CACHED_GRAPHICS_SHAPES

        robotUrdfPath = "./ur3/ur3_visual_grabbing_with_rg2.urdf"
        robotStartPos = [0, 0, 0]
        robotStartOrn = self.bullet_client.getQuaternionFromEuler([0, 0, 0])
        self.ur3 = self.bullet_client.loadURDF(robotUrdfPath, np.array(robotStartPos) + self.offset, robotStartOrn,
                                                 useFixedBase=True, flags=flags)

        self.finger_target = 0
        # create a constraint to keep the fingers centered
        jointTypeList = ["REVOLUTE", "PRISMATIC", "SPHERICAL", "PLANAR", "FIXED"]
        numJoints = self.bullet_client.getNumJoints(self.ur3)
        jointInfo = namedtuple("jointInfo",
                               ["id", "name", "type", "lowerLimit", "upperLimit", "maxForce", "maxVelocity"])
        self.joints = AttrDict()
        # get jointInfo
        for i in range(numJoints):
            self.bullet_client.changeDynamics(self.ur3, i, linearDamping=0, angularDamping=0)
            info = self.bullet_client.getJointInfo(self.ur3, i)
            jointID = info[0]
            jointName = info[1].decode("utf-8")
            jointType = jointTypeList[info[2]]
            jointLowerLimit = info[8]
            jointUpperLimit = info[9]
            jointMaxForce = info[10]
            jointMaxVelocity = info[11]
            singleInfo = jointInfo(jointID, jointName, jointType, jointLowerLimit, jointUpperLimit, jointMaxForce,
                                   jointMaxVelocity)
            self.joints[singleInfo.name] = singleInfo

    def reset(self, a=0.1, v=0.05, reset_gripper_length=0, reset_joint_position=None):
        if reset_joint_position is None:
            reset_joint_position = [0, -0.5 * math.pi, 0, 0, 0.5 * math.pi, 0]
        self.gripper_opening_length_control = p.addUserDebugParameter("gripper_opening_length", 0, 110, reset_gripper_length)
        self.position_control_group = []
        self.position_control_group.append(p.addUserDebugParameter("shoulder_pan_joint", -math.pi, math.pi, reset_joint_position[0]))
        self.position_control_group.append(p.addUserDebugParameter("shoulder_lift_joint", -math.pi, math.pi, reset_joint_position[1]))
        self.position_control_group.append(p.addUserDebugParameter("elbow_joint", -math.pi, math.pi, reset_joint_position[2]))
        self.position_control_group.append(p.addUserDebugParameter("wrist_1_joint", -math.pi, math.pi, reset_joint_position[3]))
        self.position_control_group.append(p.addUserDebugParameter("wrist_2_joint", -math.pi, math.pi, reset_joint_position[4]))
        self.position_control_group.append(p.addUserDebugParameter("wrist_3_joint", -math.pi, math.pi, reset_joint_position[5]))
        i = 0
        for jointName in self.joints:
            if jointName in position_control_joint_name:
                self.bullet_client.setJointMotorControl2(self.ur3, i, self.bullet_client.POSITION_CONTROL,rp[i])
                i += 1
            if jointName == EndEffector_joint_name:
                self.virtualGripperControl()
        self.rob.movej((rp), a, v)
        gripper = OnRobotGripperRG2(self.rob)
        gripper.open_gripper(
            target_width=reset_gripper_length,  # Width in mm, 110 is fully open
            target_force=40,  # Maximum force applied in N, 40 is maximum
            payload=0.5,  # Payload in kg
            set_payload=False,  # If any payload is attached
            depth_compensation=False,  # Whether to compensate for finger depth
            slave=False,  # Is this gripper the master or slave gripper?
            wait=2  # Wait up to 2s for movement
        )

    def virtualGripperControl(self):
        self.gripper_opening_length = p.readUserDebugParameter(self.gripper_opening_length_control)
        self.gripper_opening_angle = self.gripper_opening_length * 0.8/101
        self.bullet_client.setJointMotorControl2(self.ur3,
                                                 self.joints[gripper_main_control_joint_name].id,
                                                 self.bullet_client.POSITION_CONTROL,
                                                 targetPosition=self.gripper_opening_angle,
                                                 force=self.joints[gripper_main_control_joint_name].maxForce,
                                                 maxVelocity=self.joints[
                                                     gripper_main_control_joint_name].maxVelocity)
        for i in range(len(mimic_joint_name)):
            joint = self.joints[mimic_joint_name[i]]
            self.bullet_client.setJointMotorControl2(self.ur3, joint.id, self.bullet_client.POSITION_CONTROL,
                                                     targetPosition=self.gripper_opening_angle * mimic_multiplier[i] + mimic_offset[i],
                                                     force=10,
                                                     maxVelocity=joint.maxVelocity)

    def realGripperControl(self):
        gripper = OnRobotGripperRG2(self.rob)
        gripper.open_gripper(
            target_width=self.gripper_opening_length,  # Width in mm, 110 is fully open
            target_force=40,  # Maximum force applied in N, 40 is maximum
            payload=0.5,  # Payload in kg
            set_payload=False,  # If any payload is attached
            depth_compensation=False,  # Whether to compensate for finger depth
            slave=False,  # Is this gripper the master or slave gripper?
            wait=2  # Wait up to 2s for movement
        )

    def virtualRobotControl(self):
        self.parameter = []
        for i in range(6):
            self.parameter.append(p.readUserDebugParameter(self.position_control_group[i]))
        num = 0
        for jointName in self.joints:
            if jointName in position_control_joint_name:
                joint = self.joints[jointName]
                p.setJointMotorControl2(self.ur3, joint.id, p.POSITION_CONTROL,
                                        targetPosition=self.parameter[num],
                                        force=joint.maxForce,
                                        maxVelocity=joint.maxVelocity)
                num += 1

    def realRobotControl(self, a=0.1, v=0.05):
        time.sleep(2) # Wait up to 2s for movement
        self.rob.movej((self.parameter), a, v)

对代码有不明白的地方可以去带有onrobot_rg2夹爪的ur3机械臂在pybullet中的仿真博客和Mofeywalker的github中寻找答案。

3.连接真实机械臂

连接真实机械臂，我师兄的博客已经写了详细的教程，🔗在这里，各位可以参考我师兄的连接步骤，需要注意的是ip地址的设置。
连接完ur3机械臂后，在PyCharm中编写控制代码。先从ur3robot API中导入模块，ur3Sim(p,[0,0,0],"192.168.56.1")用来连接pybullet虚拟世界和ur3机械臂ip地址，ur3.reset()初始化机器人和夹爪的位姿，ur3.virtualRobotControl()和ur3.virtualGripperControl()控制pybullet虚拟世界中的ur3机械臂和rg2夹爪，ur3.realRobotControl()和ur3.realGripperControl()控制真实世界中的ur3机械臂和rg2夹爪。

import pybullet as p
import pybullet_data as pd
from ur3robot import ur3Sim

// video requires ffmpeg available in path
createVideo=False

// connect to engine servers
if createVideo:
    p.connect(p.GUI, options="--mp4=\"pybullet_grasp.mp4\", --mp4fps=240")
else:
    p.connect(p.GUI)

//  add search path for loadURDF
p.setAdditionalSearchPath(pd.getDataPath())
//  define world
timeStep=1./120.#240.
p.setTimeStep(timeStep)
p.setGravity(0,0,-9.8)
p.resetDebugVisualizerCamera(cameraDistance=0.7,cameraYaw=70,cameraPitch=-40,cameraTargetPosition=[0.3, 0, 0.5])
p.setRealTimeSimulation(1)

ur3 = ur3Sim(p,[0,0,0],"192.168.56.1")
ur3.reset()

while True:
    if createVideo:
        p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING,1)
    # robot control
    ur3.virtualRobotControl()
    ur3.realRobotControl()
    # gripper control
    ur3.virtualGripperControl()
    ur3.realGripperControl()

ur3机械臂和rg2夹爪在pybullet虚拟世界中运动完后，2秒之后现实世界中的ur3机械臂和rg2夹爪才会运动，如果想要提高运行速度，减小等待时间的话，可以自行在代码中进行修改。