一、今天学到了什么

核心收获：

• ✅ 理解 POSITION_CONTROL 和 VELOCITY_CONTROL 的本质区别

• ✅ 学会用正弦波让关节来回摆动

• ✅ 学会用相位偏移让四条腿交替运动

一句话总结： 机器人走路不是靠“想走”，而是靠每条腿按特定节奏摆动。

二、任务1：两种控制模式的对决

2.1 代码与注释

python

import pybullet as p
import pybullet_data
import time

p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
robot = p.loadURDF("quadruped/quadruped.urdf", [0, 0, 0.2])

hip_joint = 0  # 右前腿髋关节

# ========== 模式1：POSITION_CONTROL ==========
print("模式1：转到固定角度后锁死")
p.setJointMotorControl2(robot, hip_joint, p.POSITION_CONTROL, 
                       targetPosition=0.5, force=30.0)
for _ in range(300):
    p.stepSimulation()
    time.sleep(1/240)

# ========== 模式2：VELOCITY_CONTROL ==========
print("模式2：持续转动，像马达")
p.setJointMotorControl2(robot, hip_joint, p.VELOCITY_CONTROL, 
                       targetVelocity=2.0, force=30.0)
for _ in range(300):
    p.stepSimulation()
    time.sleep(1/240)

# ========== 停止控制 ==========
print("停止控制，关节因惯性继续运动")
p.setJointMotorControl2(robot, hip_joint, p.VELOCITY_CONTROL, 
                       targetVelocity=0, force=0)
for _ in range(300):
    p.stepSimulation()
    time.sleep(1/240)

input("按回车退出...")
p.disconnect()

2.2 动手改一改（加深理解）

改什么	怎么改	预期变化
targetPosition=0.5	改成 1.0	腿转到更大角度
targetVelocity=2.0	改成 5.0	腿转得更快
force=30.0	改成 10.0	腿转动无力，可能被重力压住

2.3 核心结论

模式	行为	适用场景
POSITION_CONTROL	转到目标角度后锁定	抓取、定点操作
VELOCITY_CONTROL	以目标速度持续转动	走路、周期性运动

三、任务2：单腿周期性摆动（正弦波入门）

3.1 代码与详细注释

python

import pybullet as p
import pybullet_data
import time
import math

p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
robot = p.loadURDF("quadruped/quadruped.urdf", [0, 0, 0.2])

hip_joint = 0  # 右前腿

print("开始周期性摆腿...")

for step in range(1000):
    # 正弦波公式：速度 = 最大值 × sin(时间)
    #   sin(时间) 在 -1 和 1 之间来回变化
    #   速度为正 → 腿正转
    #   速度为负 → 腿反转
    #   速度过零 → 腿换向
    speed = 3.0 * math.sin(step * 0.02)
    
    p.setJointMotorControl2(robot, hip_joint, p.VELOCITY_CONTROL,
                           targetVelocity=speed, force=20.0)
    
    p.stepSimulation()
    time.sleep(1/240)

input("按回车退出...")
p.disconnect()

3.2 动手改一改

修改位置	原值	改成	观察效果
3.0 * math.sin(...)	3.0	6.0	摆动幅度变大
step * 0.02	0.02	0.05	摆动频率变快
force=20.0	20.0	5.0	腿摆动无力，可能下垂

你今天的实验： 把 3.0 改成 6.0 后，腿的摆动幅度明显变大；把 0.02 改成 0.05 后，腿摆动节奏明显加快。

3.3 正弦波公式详解

text

speed = A × sin(ω × t)

A = 振幅（最大速度）→ 控制摆动幅度
ω = 角频率 → 控制摆动快慢
t = 时间（步数 × 步长）

为什么要用正弦波？ 因为走路时关节速度不是恒定的——启动时加速、中间匀速、要换向时减速。正弦波天然符合这个规律。

四、任务3：四条腿协调运动（相位偏移）

4.1 代码与详细注释

python

import pybullet as p
import pybullet_data
import time
import math

p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
robot = p.loadURDF("quadruped/quadruped.urdf", [0, 0, 0.2])

# 四条腿的髋关节索引（从任务1的关节信息中查到）
# 画图帮助理解：
#       前
#  左前(3)  右前(0)
#  左后(9)  右后(6)
#       后
hip_joints = [0, 3, 6, 9]

print("开始原地踏步...")

for step in range(2000):
    for i, hip in enumerate(hip_joints):
        # 关键：每条腿的相位不同
        # i × π/2 让每条腿错开90度相位
        # 腿0（右前）: 相位 = 时间 + 0
        # 腿1（左前）: 相位 = 时间 + 90°
        # 腿2（右后）: 相位 = 时间 + 180°
        # 腿3（左后）: 相位 = 时间 + 270°
        phase = step * 0.03 + i * math.pi / 2
        speed = 2.0 * math.sin(phase)
        
        p.setJointMotorControl2(robot, hip, p.VELOCITY_CONTROL,
                               targetVelocity=speed, force=30.0)
    
    p.stepSimulation()
    time.sleep(1/240)

input("按回车退出...")
p.disconnect()

4.2 相位偏移原理图解

text

时间 →
腿0 (右前):  ↑↓↑↓↑↓  （相位0°）
腿1 (左前):    ↑↓↑↓↑↓  （相位90°，晚1/4周期）
腿2 (右后):      ↑↓↑↓↑↓  （相位180°，晚1/2周期）
腿3 (左后):        ↑↓↑↓↑↓  （相位270°，晚3/4周期）

效果：四条腿按顺序依次摆动 → 机器人可以向前移动

4.3 动手改一改

修改位置	原值	改成	预期效果
2.0 * math.sin(...)	2.0	4.0	动作幅度变大
step * 0.03	0.03	0.06	踏步节奏变快
i * math.pi / 2	/ 2	/ 4	四腿同步摆动（像兔子跳）

你今天的观察： 把 0.03 改成 0.06 后，机器人踏步节奏明显加快；把 2.0 改成 4.0 后，腿部摆动幅度变大，机器人晃动更明显。

五、今日踩坑与解决

问题	原因	解决方案
运行后没有反应	只执行了 notepad 没执行 python	先 notepad xxx.py 编辑，再 python xxx.py 运行
机器人位置一直是0	只控制了髋关节，没有膝关节配合	需要加入膝关节的抬起/放下动作（明天学）

六、与速腾聚创课题的关联

今天学的	对应课题
VELOCITY_CONTROL + 正弦波	课题5（先进控制策略）
相位偏移 + 多腿协调	课题2（灵巧手抓取也需要多指协调）
理解关节控制底层逻辑	课题1（VLA模型最终输出关节控制信号）

七、今日学习记录

项目	内容
学习时间	3小时
完成内容	3个任务全部跑通 + 动手改参数实验
代码文件	day3_task1_control_modes.py、day3_task2_single_leg_swing.py、day3_task3_four_legs_coordination.py
关键收获	正弦波控制、相位偏移、两种控制模式的区别

明日目标：读取传感器数据，让机器人真正向前走。

八、给未来复习的自己

如果有一天我忘了这些代码是干什么的，请记住：

1. POSITION_CONTROL = 转到固定角度后锁死（适合抓取）

2. VELOCITY_CONTROL = 持续转动（适合走路）

3. 正弦波 = 让关节来回摆动的数学公式

4. 相位偏移 = 让不同关节按顺序运动的技巧

走路的核心公式：speed = A × sin(时间 × 频率 + 相位)

---

本文共3个完整代码示例，均可直接复制运行。建议边跑边改参数，观察机器人的行为变化。