PyBullet实现四足机器人A1键盘控制（前进/后退/停止）

PyBullet实现四足机器人A1键盘控制（前进/后退/停止）

一、前言

四足机器人仿真是机器人学学习的经典场景，PyBullet作为轻量级的物理仿真引擎，无需复杂配置即可快速实现机器人的动力学仿真与关节控制。本文将从零搭建A1四足机器人的仿真环境，通过键盘交互实现机器人前进、后退、停止的核心功能，帮助新手理解四足机器人步态设计、键盘监听与PyBullet仿真控制的核心逻辑。

二、核心功能与实现思路

1. 功能概述

• 基于PyBullet加载A1四足机器人URDF模型，配置物理仿真环境；
• 设计三足步态算法，实现机器人前进、后退的运动逻辑；
• 通过pynput监听键盘输入（↑/↓/空格），实时切换机器人运动状态；
• 支持仿真参数（关节最大输出力、相机距离）可视化调试，动态更新相机视角。

2. 核心实现思路

1. 环境初始化：连接PyBullet GUI、加载地面与机器人模型、配置物理参数（重力、时间步）、初始化调试参数；
2. 步态设计：定义站立姿态作为基础，通过正弦函数调制关节角度实现前进/后退的三足步态；
3. 键盘监听：以非阻塞方式监听键盘按键，通过状态变量切换机器人运动模式；
4. 主循环控制：根据当前运动状态选择对应步态，更新关节角度并推进仿真，实时刷新相机视角。

三、完整代码与逐模块解析

1. 依赖安装

首先安装运行代码所需的依赖库：

pip install pybullet numpy pynput

2. 完整代码

import pybullet as p
import time
import numpy as np
from pynput import keyboard  # 用于监听键盘输入

# ---------------------- 全局变量 ----------------------
maxForceId = None          # 关节最大力调试参数ID
cameraDistId = None        # 相机距离调试参数ID
jointIds = []              # 可控制的旋转关节ID列表
quadruped = None           # 机器人主体ID
pre_pos = []               # 轨迹记录前置位置
traceIds = [i*4+5 for i in range(4)]  # 腿部轨迹记录的Link ID

# 机器人运动状态：stop(停止)/forward(前进)/backward(后退)
robot_state = "stop"  
# 按键监听标志（避免重复触发）
pressed_keys = set()

# ---------------------- 步态函数 ----------------------
# 四足机器人12个关节默认角度（站立姿态）
def stand_pose():
    return [
        0, np.pi/4, -np.pi/2,    # 左前腿 (髋, 大腿, 小腿)
        0, np.pi/4, -np.pi/2,    # 右前腿
        0, np.pi/4, -np.pi/2,    # 左后腿
        0, np.pi/4, -np.pi/2,    # 右后腿
    ]

# 前进步态（三足步态）：对角腿同步运动
def gait_forward(phase):
    swing_amp = 0.2  # 摆动幅度
    leg1 = swing_amp * np.sin(phase)   # 左前/右后腿摆动角度
    leg2 = -swing_amp * np.sin(phase)  # 右前/左后腿摆动角度
    return [
        0, np.pi/4 + leg1, -np.pi/2 + leg1,
        0, np.pi/4 + leg2, -np.pi/2 + leg2,
        0, np.pi/4 + leg2, -np.pi/2 + leg2,
        0, np.pi/4 + leg1, -np.pi/2 + leg1,
    ]

# 后退步态（修正版）：与前进方向相反
def gait_backward(phase):
    swing_amp = 0.2
    leg1 = -swing_amp * np.sin(phase)  # 反向摆动
    leg2 = swing_amp * np.sin(phase)   
    return [
        0, np.pi/4 + leg1, -np.pi/2 - leg1,
        0, np.pi/4 + leg2, -np.pi/2 - leg2,
        0, np.pi/4 + leg2, -np.pi/2 - leg2,
        0, np.pi/4 + leg1, -np.pi/2 - leg1,
    ]

# ---------------------- 键盘监听函数 ----------------------
def on_press(key):
    """按键按下时更新机器人状态（避免重复触发）"""
    global robot_state
    try:
        if key not in pressed_keys:
            pressed_keys.add(key)
            # ↑ 前进
            if key == keyboard.Key.up:
                robot_state = "forward"
                print("\n机器人切换为：前进模式")
            # ↓ 后退
            elif key == keyboard.Key.down:
                robot_state = "backward"
                print("\n机器人切换为：后退模式")
            # 空格 停止
            elif key == keyboard.Key.space:
                robot_state = "stop"
                print("\n机器人切换为：停止模式")
    except AttributeError:
        pass

def on_release(key):
    """按键松开时移除标志（避免重复触发）"""
    try:
        if key in pressed_keys:
            pressed_keys.remove(key)
    except AttributeError:
        pass

# ---------------------- 仿真初始化 ----------------------
def load_robot():
    global maxForceId, cameraDistId, quadruped
    # 连接PyBullet GUI
    p.connect(p.GUI, options="--window_title=A1 Quadruped Control")
    # 加载地面
    plane = p.loadURDF("plane.urdf")
    # 设置物理参数
    p.setGravity(0, 0, -9.8)
    p.setTimeStep(1./200)
    p.setPhysicsEngineParameter(fixedTimeStep=1./200)

    # 加载A1机器人（兼容不同URDF路径）
    urdfFlags = p.URDF_USE_SELF_COLLISION
    try:
        quadruped = p.loadURDF("a1/urdf/a1.urdf", [0, 0, 0.45], [0,0,0,1], 
                               flags=urdfFlags, useFixedBase=False)
    except:
        quadruped = p.loadURDF("urdf/a1.urdf", [0, 0, 0.45], [0,0,0,1], 
                               flags=urdfFlags, useFixedBase=False)

    # 初始化轨迹记录
    for i in traceIds:
        pre_pos.append(p.getLinkState(quadruped, i)[0])

    # 启用腿部自碰撞
    lower_legs = [2,5,8,11]
    for l0 in lower_legs:
        for l1 in lower_legs:
            if l1>l0:
                p.setCollisionFilterPair(quadruped, quadruped, l0, l1, 1)

    # 创建调试参数（GUI滑动条）
    maxForceId = p.addUserDebugParameter("maxForce", 0, 200, 80)
    cameraDistId = p.addUserDebugParameter("cameraDist", 0, 5, 2.0)

    # 收集可控制的旋转关节
    for j in range(p.getNumJoints(quadruped)):
        p.changeDynamics(quadruped, j, linearDamping=0.01, angularDamping=0.01)
        info = p.getJointInfo(quadruped, j)
        if info[2] == p.JOINT_REVOLUTE:
            jointIds.append(j)
    
    assert len(jointIds) == 12, f"错误：只识别到{len(jointIds)}个关节，应为12个！"
    p.setRealTimeSimulation(0)

# ---------------------- 关节控制 ----------------------
def set_joints(target_angles):
    """设置关节角度并推进仿真"""
    maxForce = p.readUserDebugParameter(maxForceId)
    for i in range(12):
        p.setJointMotorControl2(
            bodyUniqueId=quadruped,
            jointIndex=jointIds[i],
            controlMode=p.POSITION_CONTROL,
            targetPosition=target_angles[i],
            force=maxForce
        )
    # 推进仿真一步
    p.stepSimulation()

    # 动态更新相机视角（跟随机器人主体）
    pos, _ = p.getBasePositionAndOrientation(quadruped)
    dist = p.readUserDebugParameter(cameraDistId)
    p.resetDebugVisualizerCamera(
        cameraDistance=dist,
        cameraYaw=50,
        cameraPitch=-35,
        cameraTargetPosition=pos
    )
    time.sleep(1./200)

# ---------------------- 主程序 ----------------------
if __name__ == '__main__':
    # 1. 初始化仿真环境
    load_robot()
    phase = 0.0  # 步态相位（控制正弦摆动）

    # 2. 启动键盘监听（非阻塞模式）
    listener = keyboard.Listener(on_press=on_press, on_release=on_release)
    listener.start()
    print("="*50)
    print("机器人控制说明：")
    print("↑ 方向键：前进")
    print("↓ 方向键：后退")
    print("空格键：停止")
    print("="*50)

    # 3. 先让机器人稳定站立2秒（400步×5ms=2000ms）
    print("\n机器人正在站立...")
    for _ in range(400):
        set_joints(stand_pose())

    # 4. 主循环：根据状态控制机器人
    print("\n等待键盘指令...")
    while True:
        # 根据当前状态选择对应的步态
        if robot_state == "stop":
            target_angles = stand_pose()  # 停止时保持站立姿态
        elif robot_state == "forward":
            phase += 0.08  # 相位递增，驱动步态循环
            target_angles = gait_forward(phase)
        elif robot_state == "backward":
            phase += 0.08
            target_angles = gait_backward(phase)
        
        # 执行关节控制与仿真推进
        set_joints(target_angles)

        # 实时打印机器人状态与位置
        pos, _ = p.getBasePositionAndOrientation(quadruped)
        print(f"\r当前状态：{robot_state} | 位置：x={pos[0]:.2f}, y={pos[1]:.2f}", end="")

3. 关键模块解析

（1）全局变量定义

核心全局变量用于存储仿真核心对象（机器人ID、关节ID）、调试参数ID、运动状态与键盘监听标志，其中pressed_keys集合用于避免按键重复触发，保证状态切换的稳定性。

（2）步态函数设计

• 站立姿态 ：stand_pose()定义12个关节的初始角度（四足各3个关节：髋、大腿、小腿），使机器人保持稳定站立；
• 前进步态 ：gait_forward()采用三足步态逻辑，通过正弦函数np.sin(phase)调制对角腿的摆动角度，实现"左前+右后"与"右前+左后"腿交替摆动；
• 后退步态 ：gait_backward()反向调制正弦角度，实现与前进相反的运动方向。

（3）键盘监听逻辑

通过pynput.keyboard.Listener创建非阻塞键盘监听线程：

• on_press()：按键按下时更新运动状态（↑=前进、↓=后退、空格=停止），并通过pressed_keys避免重复触发；
• on_release()：按键松开时移除标志，保证下次按键可正常响应。

（4）仿真初始化

load_robot()完成核心初始化工作：

• 连接PyBullet GUI并配置物理参数（重力、时间步）；
• 加载地面与A1机器人（兼容不同URDF文件路径）；
• 启用腿部自碰撞，避免运动时腿部穿透；
• 创建GUI调试滑动条（最大关节力、相机距离）；
• 筛选并收集12个旋转关节ID，确保关节控制的准确性。

（5）关节控制与主循环

• set_joints()：读取调试参数（最大关节力），通过p.setJointMotorControl2实现关节位置控制，推进仿真并动态更新相机视角（跟随机器人主体）；
• 主循环：先让机器人稳定站立2秒，再根据robot_state切换步态，通过phase递增驱动步态循环，实时打印机器人位置与状态。

四、运行与调试说明

1. 运行条件

• 确保A1机器人的URDF文件路径正确（代码兼容a1/urdf/a1.urdf和urdf/a1.urdf两种路径）；
• 运行代码后会弹出PyBullet GUI窗口，同时控制台输出控制说明。

2. 交互控制

• 按↑方向键：机器人切换为前进模式，执行前进步态；
• 按↓方向键：机器人切换为后退模式，执行后退步态；
• 按空格键：机器人切换为停止模式，恢复站立姿态；
• 通过GUI滑动条可调整maxForce（关节输出力）和cameraDist（相机距离），优化运动效果与视角。

3. 常见问题解决

• 关节数量错误：检查URDF文件是否完整，确保12个旋转关节被正确识别；
• 机器人摔倒：增大maxForce值（如调整至100以上），或减小步态摆动幅度swing_amp；
• 键盘无响应：确保焦点在控制台窗口，而非PyBullet GUI窗口。

五、扩展方向

1. 步态优化：引入PID控制优化关节角度跟踪精度，或设计更复杂的四足步态（如 trot 步态）；
2. 避障功能：添加激光雷达/视觉传感器，实现障碍物检测与避障；
3. 速度控制：通过键盘数字键调节步态相位递增速度，实现运动速度可调；
4. 姿态稳定：读取机器人IMU数据，闭环控制机身姿态（如保持水平）。

六、总结

本文基于PyBullet实现了A1四足机器人的键盘交互控制，核心在于步态函数的设计、键盘监听的非阻塞实现与PyBullet的关节位置控制。通过本文的代码与解析，新手可快速掌握四足机器人仿真的核心逻辑，为后续更复杂的机器人控制算法开发打下基础。