欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页  >  IT编程

带有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指相机的焦距的变化,类似变焦.
然后,就是导入机器人模型,robotStartPosrobotStartOrn表示的是机器人起始时的位姿.

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模块,namedtupleAttrDict.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 value=multiplier∗otherjointvalue+offset value=multiplierotherjointvalue+offset

  • 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