实时控制机器人手臂参数（以UR5机器人手臂为例）

系列文章目录：
上一篇：
PyBullet（七）在PyBullet中使用VR
下一篇：

系统：Win10
PyBullet：（官方链接）（官方指南）


1. 整体思路

没啥思路，直接开整！


2. 代码解析


2.1 代码分步


2.1.1 库

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

最基础的导入包啊，库啊。没有安装的可以自行pip/pip3 install ......。


2.1.2 连接物理模拟

physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-9.81)#设置重力

导入PyBullet模块后，首先要做的是 "连接 "物理模拟。PyBullet是围绕客户端-服务器驱动的API设计的，客户端发送命令，物理服务器返回状态。PyBullet有一些内置的物理服务器。DIRECT和GUI。GUI和DIRECT连接都会在与PyBullet相同的过程中执行物理模拟和渲染。

至于p.setAdditionalSearchPath(pybullet_data.getDataPath())你可以提供你自己的数据文件，也可以使用PyBullet附带的PyBullet_data包。为此，导入 pybullet_data 并使用pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())注册该目录。


2.1.3 “固定视角”

哈哈哈哈哈哈不是不能改变视角，而是在每次启动程序的时候都能是固定视角。

p.resetDebugVisualizerCamera(cameraDistance=2,cameraYaw=0,cameraPitch=-40,cameraTargetPosition=[0.5,-0.9,0.5])#转变视角

2.1.4 加载模型

planeId = p.loadURDF("plane.urdf")
robotId = p.loadURDF("D:/Work/Internship/porject/object/UR5.urdf",useFixedBase = True)

记得要更改模型加载的地址！

模型链接：PyBullet（六）UR5机器人手臂模型


2.1.5 登记各个节点的信息

#记录各个节点类型的列表
jointTypeList = ["REVOLUTE", "PRISMATIC", "SPHERICAL", "PLANAR", "FIXED"]
#得到机器人的节点个数
numJoints = p.getNumJoints(robotId)
#属于提前创造存储节点信息的数据结构
jointInfo = namedtuple("jointInfo",["id","name","type","lowerLimit","upperLimit","maxForce","maxVelocity"])
#另一个数据结构
joints = AttrDict()
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

可以打印出来看看里面的内容，不会用函数的可以上网查一查，这里就不再赘述了。

print(joints)

2.1.6 添加界面右边的参数

图1

界面示例

#创建空列表
position_control_group = []
#将创建的参数存到空列表中
#参数：["节点名字","可滑动最小的角度","可滑动最大的角度","节点初始角度"]
position_control_group.append(p.addUserDebugParameter('joint0', -math.pi, math.pi, 0))
position_control_group.append(p.addUserDebugParameter('joint1', -2.0 / 3 * math.pi, -1.0 / 3 * math.pi, -0.5 * math.pi))
position_control_group.append(p.addUserDebugParameter('joint2', -0.5 * math.pi, 0.5 * math.pi, 0))
position_control_group.append(p.addUserDebugParameter('joint3', -math.pi, 0, -0.5 * math.pi))
position_control_group.append(p.addUserDebugParameter('joint4', -0.5 * math.pi, 0.5 * math.pi, 0))
position_control_group.append(p.addUserDebugParameter('joint5', -math.pi, math.pi, 0))

2.1.7 代码主体（呈现“参数”）

position_control_joint_name = ["joint0","joint1","joint2","joint3","joint4","joint5"]
print("position_control_group:",position_control_group)
while True:
    time.sleep(0.01)
    parameter = []
    #将添加的参数“读”出来
    for i in range(6):
        parameter.append(p.readUserDebugParameter(position_control_group[i]))
    num = 0
    print("parameter:",parameter)
    for jointName in joints:
        if jointName in position_control_joint_name:
            joint = joints[jointName]
            parameter_sim = parameter[num]
            #机器人手臂的移动
            #可参考[PyBullet (二) 机器人手臂的简单移动]
            #(https://blog.csdn.net/xzs1210652636/article/details/109184775#t11)中的setJointMotorControlArray
            p.setJointMotorControl2(robotId, joint.id, p.POSITION_CONTROL,
                                    targetPosition=parameter_sim,
                                    force=joint.maxForce,
                                    maxVelocity=joint.maxVelocity)
            num = num + 1
    p.stepSimulation()

大家运行的时候可以把“print”删掉，这个是我一开始测试用的。


2.2 代码总体

# -*- coding: utf-8 -*-
"""
Created on Wed Mar 10 10:57:49 2021

@author: 12106
"""


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

physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-9.81)

p.resetDebugVisualizerCamera(cameraDistance=2,cameraYaw=0,cameraPitch=-40,cameraTargetPosition=[0.5,-0.9,0.5])#转变视角


planeId = p.loadURDF("plane.urdf")
robotId = p.loadURDF("D:/Work/Internship/porject/object/UR5.urdf",useFixedBase = True)

#登记各个节点的信息
jointTypeList = ["REVOLUTE", "PRISMATIC", "SPHERICAL", "PLANAR", "FIXED"]
numJoints = p.getNumJoints(robotId)
jointInfo = namedtuple("jointInfo",["id","name","type","lowerLimit","upperLimit","maxForce","maxVelocity"])
joints = AttrDict()
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

print(joints)

for jointName in joints:
    print("jointName:",jointName)
    
    
position_control_group = []
position_control_group.append(p.addUserDebugParameter('joint0', -math.pi, math.pi, 0))
position_control_group.append(p.addUserDebugParameter('joint1', -2.0 / 3 * math.pi, -1.0 / 3 * math.pi, -0.5 * math.pi))
position_control_group.append(p.addUserDebugParameter('joint2', -0.5 * math.pi, 0.5 * math.pi, 0))
position_control_group.append(p.addUserDebugParameter('joint3', -math.pi, 0, -0.5 * math.pi))
position_control_group.append(p.addUserDebugParameter('joint4', -0.5 * math.pi, 0.5 * math.pi, 0))
position_control_group.append(p.addUserDebugParameter('joint5', -math.pi, math.pi, 0))

position_control_joint_name = ["joint0","joint1","joint2","joint3","joint4","joint5"]
print("position_control_group:",position_control_group)
while True:
    time.sleep(0.01)
    #p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)  # 允许机械臂慢慢渲染
    
    parameter = []
    for i in range(6):
        parameter.append(p.readUserDebugParameter(position_control_group[i]))
    num = 0
    print("parameter:",parameter)
    for jointName in joints:
        if jointName in position_control_joint_name:
            joint = joints[jointName]
            parameter_sim = parameter[num]
            p.setJointMotorControl2(robotId, joint.id, p.POSITION_CONTROL,
                                    targetPosition=parameter_sim,
                                    force=joint.maxForce,
                                    maxVelocity=joint.maxVelocity)
            num = num + 1
    p.stepSimulation()

3. 效果展示

视频演示（Bilibili）
视频演示（YouTube）（暂无）


4. 总结

这一段代码的目的是为了让我们更加方便的调试机器人上手臂，让我们共同进步！