pybullet | 机械臂招手仿真

import pybullet as p                 # 导入 PyBullet 库进行物理仿真
import pybullet_data                 # 导入 PyBullet 数据库
import math                          # 导入数学库用于计算角度
import time                          # 导入时间库用于控制程序运行时间

# 初始化 PyBullet 物理环境
physicsClient = p.connect(p.GUI)      # 创建 PyBullet 物理环境，并以图形用户界面(GUI)的形式显示

# 加载机械臂模型
jaka = p.loadURDF(r"pu\jaka_description\urdf\jaka_description.urdf", useFixedBase=True)     # 加载 URDF 格式的机械臂模型，模型路径可以根据实际情况修改

# 设置重力
p.setGravity(0, 0, -9.81)            # 设置物理环境的重力加速度，参数为x,y,z轴的重力

# 设置初始关节角度、关节运动范围、招手的角度范围和速度
initial_joint_positions = [0, math.pi/4, math.pi/2, 0, 0, 0]                             # 机械臂的初始关节角度
joint_limits = [(-math.pi, math.pi), (-math.pi/4, math.pi/4), (-math.pi/4, math.pi/4),   # 机械臂各个关节的角度范围
                (-math.pi, math.pi), (-math.pi, math.pi), (-math.pi, math.pi)]
wave_angle_range = [0, math.pi/4]    # 招手动作的角度范围
wave_speed = 0.01                    # 招手的速度

# 开始运动
count = 0                            # 计数器，记录招手运动循环次数
max_count = 1000                     # 招手运动循环的最大次数

while count < max_count:              # 招手运动循环次数未达到最大次数时
    for t in range(int(1/wave_speed)):       # 在一段时间内进行多个招手动作
        angle = (wave_angle_range[1] - wave_angle_range[0]) * math.sin(math.pi * 2 * t * wave_speed) / 2 + (wave_angle_range[1] + wave_angle_range[0]) / 2     # 计算当前招手动作的角度
        wave_joint_positions = initial_joint_positions.copy()     # 复制初始关节角度列表，用于修改当前时刻的关节角度

        wave_joint_positions[3] = angle        # 设置第4个关节的角度为招手角度
        wave_joint_positions[4] = -angle       # 设置第5个关节的角度为招手角度的负值

        for j in range(6):                      # 遍历机械臂的6个关节
            wave_joint_positions[j] = max(joint_limits[j][0], wave_joint_positions[j])     # 确保关节角度在范围内
            wave_joint_positions[j] = min(joint_limits[j][1], wave_joint_positions[j])

            p.setJointMotorControl2(jaka, j, p.POSITION_CONTROL, wave_joint_positions[j], force=50)    # 控制机械臂的关节运动

        p.stepSimulation()                # 更新物理环境
        time.sleep(0.01)                  # 控制程序运行时间

    count += 1                           # 招手运动循环次数加1

# 停止机械臂运动
for i in range(6):                        # 遍历机械臂的6个关节
    p.setJointMotorControl2(jaka, i, p.POSITION_CONTROL, initial_joint_positions[i], force=50)    # 控制机械臂的关节停止运动

# 更新 PyBullet 物理环境
p.stepSimulation()
time.sleep(0.01)

# 断开 PyBullet 物理环境
p.disconnect()                          # 断开与物理仿真环境的连接