实时控制机器人手臂参数(以UR5机器人手臂为例)
系列文章目录:
上一篇:
PyBullet(七)在PyBullet中使用VR
下一篇:
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)
记得要更改模型加载的地址!
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 添加界面右边的参数
#创建空列表
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. 总结
这一段代码的目的是为了让我们更加方便的调试机器人上手臂,让我们共同进步!
评论(0)
您还未登录,请登录后发表或查看评论