pybullet学习——ur3机器人

pybullet学习——ur3机器人

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

p.connect(p.GUI)
p.resetDebugVisualizerCamera(cameraDistance=1.2,cameraYaw=0,\
                             cameraPitch=-20,cameraTargetPosition=[0.1,-0.35,0.2]) # 视角，cameraYaw 横向角度，cameraPitch仰角
p.setAdditionalSearchPath(pd.getDataPath())

######### 载入模型
pandaUid=p.loadURDF("UR34/UR34.urdf",useFixedBase=True)
tableUid=p.loadURDF("table/table.urdf",basePosition=[0,0,-0.6])
trayUid=p.loadURDF("tray/traybox.urdf",basePosition=[0.6,0,0.05])

######### 对机器人参数读取
numJoints = p.getNumJoints(pandaUid)
print('机器人关节数目: %d'%numJoints)
jointTypeList = ["REVOLUTE", "PRISMATIC", "SPHERICAL", "PLANAR", "FIXED"] # 定义机器人关节类型数组
jointInfo = namedtuple("jointInfo", # namedtuple: 创建一个具名元组，需要两个参数，一个是类名
                       ["id","name","type","lowerLimit","upperLimit","maxForce","maxVelocity"]) # 定义数组存放机器人关节信息
joints = AttrDict() # 就是字典，但可以通过 aa.xx来得到字典内容
for i in range(numJoints):
    info = p.getJointInfo(pandaUid, i) 
    jointID = info[0] # jointIndex
    jointName = info[1].decode("utf-8") # jointName 读出来是2进制，需要转
    jointType = jointTypeList[info[2]] # jointType 
    jointLowerLimit = info[8] # jointLowerLimit 滑块和旋转（铰链）关节的位置下限
    jointUpperLimit = info[9] # jointUpperLimit 滑块和旋转（铰链）关节的位置上限
    jointMaxForce = info[10] # jointMaxForce URDF（可能其他文件格式）中指定的最大力
    jointMaxVelocity = info[11] # jointMaxVelocity URDF中指定的最大速度。
    singleInfo = jointInfo(jointID, jointName, jointType, jointLowerLimit, jointUpperLimit, jointMaxForce, jointMaxVelocity)
    joints[singleInfo.name] = singleInfo

########## 添加滑块
position_control_joint_name = ["joint1",
                            	"joint2",
                           		"joint3",
                               	"joint4",
                                "joint5",
                                "joint6"]
position_control_group = []
position_control_group.append(p.addUserDebugParameter('joint1', -math.pi, math.pi, 0))
position_control_group.append(p.addUserDebugParameter('joint2', -math.pi, math.pi, -math.pi/2))
position_control_group.append(p.addUserDebugParameter('joint3', -math.pi, math.pi, 0))
position_control_group.append(p.addUserDebugParameter('joint4', -math.pi, math.pi, -math.pi/2))
position_control_group.append(p.addUserDebugParameter('joint5', -math.pi, math.pi, 0))
position_control_group.append(p.addUserDebugParameter('joint6', -math.pi, math.pi, 0))




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]
            if joint.id == 1 or joint.id == 3:
                parameter[num] = parameter[num] + math.pi/2
            p.setJointMotorControl2(pandaUid, joint.id, p.POSITION_CONTROL, # setJointMotorControl2: p.POSITION_CONTROL 对特定关节转动多少角度
                                    targetPosition=parameter[num],
                                    force=joint.maxForce,
                                    maxVelocity=joint.maxVelocity)
            num += 1
    p.stepSimulation()

pybullet手册英文pdf：https://download.csdn.net/download/qq_42236622/85061623

ur3、table、traybox的urdf：https://download.csdn.net/download/qq_42236622/85061640

参考：https://blog.csdn.net/AIRMAN12306/article/details/109627858/