利用pybullet 实现ur5 仿真

想做一个机械臂仿真软件，以前利用C++ 结合FCL和OSG实现了一个半成品，但是这种方案需要根据机械臂DH参数，解析出机械臂各关节位姿然后利用OSG渲染，并且需要自己实现逆解算法，过于麻烦了。而使用pybullet，上述得大部分功能都有现成的API接口。

仿真软件得几个要素：

（1），渲染引擎 ，需要将模型渲染可视化出来。VTK,OSG或者原生OpenGL。

（2），动力学引擎，根据物体的物理属性计算运动、旋转和碰撞。Bullet或者ODE，或者只用于碰撞检测的FCL。

（3），模型描述，负责机器人模型的建模，描述各机器人的连杆和关节，还有质量属性，颜色等。比如常用的URDF文件和SDF文件。

pybullet 具备了上述所有的要素，动力学引擎使用bullet，渲染使用OpenGL，并可以直接加载URDF文件，而且提供了 正逆算法。

实现逻辑：

1，利用URDF文件，加载模型。

2，给出目标，也就是抓取点位置。

3，利用逆解算法，求出各关节角度。

4，设置各关节角度，并判单是否碰撞。

代码:

import pybullet as p
import pybullet_data
import time
import math
from collections   import namedtuple
import numpy as np

# 启动仿真引擎的GUI
p.connect(p.GUI)

# 设置重力加速度
p.setGravity(0, 0, -9.81)

# 加载URDF模型路径
p.setAdditionalSearchPath(pybullet_data.getDataPath())

# 设置视角
p.resetDebugVisualizerCamera(cameraDistance=3,
                                    cameraYaw=120,
                                    cameraPitch=-45,
                                    cameraTargetPosition=[0,0,0])

# 加载平面模型作为地面
planeId = p.loadURDF("plane.urdf")

#托盘
trayId=p.loadURDF("tray/traybox.urdf",basePosition=[0.6,0,0.63])

#桌子
tableId=p.loadURDF("table/table.urdf",basePosition=[0.5,0,0])

# 加载ur5 机械臂
robotId  = p.loadURDF("D:\\keiler\\sim\\urdfs\\ur5.urdf",useFixedBase=True )  #basePosition=[0.0,0,0.62]


initial_position = [0, 0, 0.62]  # 设置初始位置为 [0, 0, 0.62] 米
initial_orientation = p.getQuaternionFromEuler([0, 0, 0])  # 设置初始方向为欧拉角 [0, 0, 0] 对应的四元数
p.resetBasePositionAndOrientation(robotId, initial_position, initial_orientation)

# 获取机械臂末端执行器的索引
endEffectorIndex =6

# 获取机械臂的关节数量
numJoints = p.getNumJoints(robotId)
print("关节数量:"+str(numJoints))

# 打印每个关节的信息
for joint_index in range(numJoints):
    joint_info = p.getJointInfo(robotId, joint_index)
    print(f"Joint {joint_index}: {joint_info}")

# 机械臂的初始位置
restingPosition = [0,3.14, -1.57, 1.57, 1.57, 1.57, -1.57, 0]
for jointNumber in range(numJoints):
    p.resetJointState(robotId, jointNumber, restingPosition[jointNumber])
    
    
# 设置圆形的参数
circle_radius = 3
circle_center = [0, 0]
numPoints = 50
angles = [2 * math.pi * float(i) / numPoints for i in range(numPoints)]

#调参控件
target_x = p.addUserDebugParameter("Target X", -10, 10, 0)
target_y = p.addUserDebugParameter("Target Y", -10, 10, 0)
target_z = p.addUserDebugParameter("Target Z", -10, 10, 0)

#开始按钮
button_start = p.addUserDebugParameter("Start", 1, 0, 1)
# 初始状态变量
button_state_prev = 0


#画个球
sphere_visual = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=0.03, rgbaColor=[1, 0, 0, 1])
sphere_id = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=-1, baseVisualShapeIndex=sphere_visual, basePosition=[0, 0, 1])


#多组解
def calculate_ik_multiple_solutions(robot_id, end_effector_index, target_position, target_orientation, num_solutions=5):
    solutions = []
    for i in range(num_solutions):
        # 生成一个随机的初始关节状态
        random_joint_positions = [np.random.uniform(-np.pi, np.pi) for _ in range(numJoints)]
        # 计算逆运动学
        ik_solution = p.calculateInverseKinematics(robot_id, end_effector_index, target_position, target_orientation, jointDamping=[0.01]*numJoints, lowerLimits=[-np.pi]*numJoints, upperLimits=[np.pi]*numJoints, jointRanges=[2*np.pi]*numJoints, restPoses=random_joint_positions)
        solutions.append(ik_solution)
    return solutions


try:
    while True:
        x = p.readUserDebugParameter(target_x)
        y = p.readUserDebugParameter(target_y)
        z = p.readUserDebugParameter(target_z)
    
        # 更新球的位置
        p.resetBasePositionAndOrientation(sphere_id, [x, y, z], [0, 0, 0, 1])
        
        #判断按钮状态
        button_state = p.readUserDebugParameter(button_start)
        if button_state != button_state_prev:
            print("Button pressed")
            # 使用逆向运动学计算关节角度
            jointPoses = p.calculateInverseKinematics(robotId, endEffectorIndex, [x, y, z],[0, 0, 0, 1])
            
            # 移动机械臂
            p.setJointMotorControl2(bodyIndex=robotId,jointIndex=1,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[0],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
            p.setJointMotorControl2(bodyIndex=robotId,jointIndex=2,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[1],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
            p.setJointMotorControl2(bodyIndex=robotId,jointIndex=3,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[2],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
            p.setJointMotorControl2(bodyIndex=robotId,jointIndex=4,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[3],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
            p.setJointMotorControl2(bodyIndex=robotId,jointIndex=5,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[4],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
            p.setJointMotorControl2(bodyIndex=robotId,jointIndex=6,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[5],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
        button_state_prev = button_state

        
            
        contact_points  = p.getContactPoints(bodyA=trayId,bodyB=robotId)
        isColl =  bool(contact_points)
        if isColl:
            print("撞上了！")
        
                

        p.stepSimulation()
        time.sleep(0.01)

except KeyboardInterrupt:
    # 用户中断程序时，退出循环
    print("Circle drawing interrupted by user.")
# 断开与仿真引擎的连接
p.disconnect()

效果及源码：

利用pybullet 实现ur5 机械臂仿真 - Qt hello