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

程序员文章站 2022-04-27 10:43:08

Simulation of ur3 with onrobot_rg2 by pybulletimport pybullet as pimport pybullet_data as pd// connect to engine serversphysicsClient = p.connect(p.GUI)// add search path for loadURDFp.setAdditionalSearchPath(pd.getDataPath())p.connect(p.GUI)用来启动仿真...

Simulation of ur3 with onrobot_rg2 by pybullet

本文对带有onrobot_rg2的ur3机械臂在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 = info[10]
    jointMaxVelocity = info[11]
    singleInfo = jointInfo(jointID, jointName, jointType, jointLowerLimit, jointUpperLimit, jointMaxForce, jointMaxVelocity)
    joints[singleInfo.name] = singleInfo

在Gazebo中仿真机械手爪需要用到< mimic>标签，ROS URDF中关节标签< mimic>可以只驱动一根连杆，其它连杆模仿被驱动的连杆，这样就能控制整个手爪，官方文件中是这样描述的，< mimic>用来描述现在定义的关节与已经存在的关节的关系，其值可以用下式计算:
$v a l u e = m u l t i p l i e r * o t h e r j o i n t v a l u e + o f f s e t$

joint (required)
被模仿的关节名称
multiplier (optional)
乘积因子的数值，默认1
offset (optional)
偏置量，默认0
但是在pybullet世界中这个标签不起作用，所以只能用python代码实现相同的效果.
gripper_main_control_joint_name是被模仿的关节名称，mimic_joint_name是需要模仿的关节组，mimic_multiplier是乘积因子，mimic_offset是偏置量.

gripper_main_control_joint_name = "gripper_finger1_joint"
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]

我们通过p.addUserDebugParameter()这个api添加自定义滑块来调整rg2手爪的参数.我们可以使用readUserDebugParameter()读取参数的值.

gripper_opening_angle_control = p.addUserDebugParameter("gripper_opening_angle",0,0.8,0)

与上面的代码类似，我们再次通过p.addUserDebugParameter()这个api添加自定义滑块来调整ur3各关节的角度参数.

# joint position control
position_control_group = []
position_control_group.append(p.addUserDebugParameter('shoulder_pan_joint', -math.pi, math.pi, 0))
position_control_group.append(p.addUserDebugParameter('shoulder_lift_joint', -math.pi, math.pi, -math.pi / 2))
position_control_group.append(p.addUserDebugParameter('elbow_joint', -math.pi, math.pi, 0))
position_control_group.append(p.addUserDebugParameter('wrist_1_joint', -math.pi, math.pi, 0))
position_control_group.append(p.addUserDebugParameter('wrist_2_joint', -math.pi, math.pi, math.pi / 2))
position_control_group.append(p.addUserDebugParameter('wrist_3_joint', -math.pi, math.pi, 0))

position_control_joint_name = ["shoulder_pan_joint",
                            									   "shoulder_lift_joint",
                           									       "elbow_joint",
                               								       "wrist_1_joint",
                                                                   "wrist_2_joint",
                                                                   "wrist_3_joint"]

下面，我们来控制机械臂运动.控制电机的API是setJointMotorControl2().joint.id是机械臂关节的序号，p.POSITION_CONTROL是位置控制模式，targetPosition是想要达到的目标位置，具体值由自定义滑块控制.

while True:
    time.sleep(0.01)
    p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)  // 允许机械臂慢慢渲染
    // ur3 control
    parameter = []
    for i in range(6):
        parameter.append(p.readUserDebugParameter(position_control_group[i]))
    num = 0
    for jointName in joints:
        if jointName in position_control_joint_name:
            joint = joints[jointName]
            p.setJointMotorControl2(robotID, joint.id, p.POSITION_CONTROL,
                                    targetPosition=parameter[num],
                                    force=joint.maxForce,
                                    maxVelocity=joint.maxVelocity)
            num += 1

    // gripper control
    gripper_opening_angle = p.readUserDebugParameter(gripper_opening_angle_control)

    p.setJointMotorControl2(robotID,
                            joints[gripper_main_control_joint_name].id,
                            p.POSITION_CONTROL,
                            targetPosition=gripper_opening_angle,
                            force=joints[gripper_main_control_joint_name].maxForce,
                            maxVelocity=joints[gripper_main_control_joint_name].maxVelocity)
    for i in range(len(mimic_joint_name)):
        joint = joints[mimic_joint_name[i]]
        p.setJointMotorControl2(robotID, joint.id, p.POSITION_CONTROL,
                                targetPosition=gripper_opening_angle * mimic_multiplier[i]+mimic_offset[i],
                                force=joint.maxForce,
                                maxVelocity=joint.maxVelocity)
    p.stepSimulation()

完整代码：

import pybullet as p
import pybullet_data as pd
from collections import namedtuple
from attrdict import AttrDict
import math
import time

# connect to engine servers
physicsClient = p.connect(p.GUI)
# add search path for loadURDF
p.setAdditionalSearchPath(pd.getDataPath())
# define world
p.setGravity(0,0,-9.8)
p.resetDebugVisualizerCamera(cameraDistance=2,cameraYaw=0,cameraPitch=-40,cameraTargetPosition=[0.5,-0.9,0.5])

#######################################
###    define and setup robot       ###
#######################################
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)
# 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])

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 = info[10]
    jointMaxVelocity = info[11]
    singleInfo = jointInfo(jointID, jointName, jointType, jointLowerLimit, jointUpperLimit, jointMaxForce, jointMaxVelocity)
    joints[singleInfo.name] = singleInfo

gripper_main_control_joint_name = "gripper_finger1_joint"
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]

gripper_opening_angle_control = p.addUserDebugParameter("gripper_opening_angle",0,0.8,0)

# joint position control
position_control_group = []
position_control_group.append(p.addUserDebugParameter('shoulder_pan_joint', -math.pi, math.pi, 0))
position_control_group.append(p.addUserDebugParameter('shoulder_lift_joint', -math.pi, math.pi, -math.pi / 2))
position_control_group.append(p.addUserDebugParameter('elbow_joint', -math.pi, math.pi, 0))
position_control_group.append(p.addUserDebugParameter('wrist_1_joint', -math.pi, math.pi, 0))
position_control_group.append(p.addUserDebugParameter('wrist_2_joint', -math.pi, math.pi, math.pi / 2))
position_control_group.append(p.addUserDebugParameter('wrist_3_joint', -math.pi, math.pi, 0))

position_control_joint_name = ["shoulder_pan_joint",
                            									   "shoulder_lift_joint",
                           									       "elbow_joint",
                               								       "wrist_1_joint",
                                                                   "wrist_2_joint",
                                                                   "wrist_3_joint"]

while True:
    time.sleep(0.01)
    p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)  # 允许机械臂慢慢渲染
    # ur3 control
    parameter = []
    for i in range(6):
        parameter.append(p.readUserDebugParameter(position_control_group[i]))
    num = 0
    for jointName in joints:
        if jointName in position_control_joint_name:
            joint = joints[jointName]
            p.setJointMotorControl2(robotID, joint.id, p.POSITION_CONTROL,
                                    targetPosition=parameter[num],
                                    force=joint.maxForce,
                                    maxVelocity=joint.maxVelocity)
            num += 1

    # gripper control
    gripper_opening_angle = p.readUserDebugParameter(gripper_opening_angle_control)

    p.setJointMotorControl2(robotID,
                            joints[gripper_main_control_joint_name].id,
                            p.POSITION_CONTROL,
                            targetPosition=gripper_opening_angle,
                            force=joints[gripper_main_control_joint_name].maxForce,
                            maxVelocity=joints[gripper_main_control_joint_name].maxVelocity)
    for i in range(len(mimic_joint_name)):
        joint = joints[mimic_joint_name[i]]
        p.setJointMotorControl2(robotID, joint.id, p.POSITION_CONTROL,
                                targetPosition=gripper_opening_angle * mimic_multiplier[i]+mimic_offset[i],
                                force=joint.maxForce,
                                maxVelocity=joint.maxVelocity)
    p.stepSimulation()

参考网址：
[1]: https://blog.csdn.net/bornfree5511/article/details/108307638
[2]: https://github.com/matafela/pybullet_grasp_annotator_robotiq_85.git

本文地址：https://blog.csdn.net/AIRMAN12306/article/details/109627858

相关标签： pybullet学习机器学习强化学习仿真器

上一篇： BBSXP2008存在后台注射漏洞

下一篇： MySQL常用语句复习