带有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 = 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=multiplier∗otherjointvalue+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