Từ PD giữ dáng đến WBID có tiếp xúc

Vì sao cần bài này?

Ở bài 1, ta đã dựng Unitree G1 trong MuJoCo qua low-level DDS. Ở bài 2, ta chuyển dữ liệu motion thành MCAP để xem trong Foxglove. Ở bài 3, ta nhìn sâu hơn vào các tín hiệu MPC/WBID bằng PlotJuggler: CoM, contact force, feet reference và wbid_solve_time.

Bài 4 đi vào lõi điều khiển: tại sao một controller PD rất ngắn có thể giữ robot ở pose ban đầu, nhưng lại không đủ để đi bộ; và tại sao locomotion stack phải dùng Whole-Body Inverse Dynamics (WBID) có ràng buộc tiếp xúc. Nguồn chính của bài là repo ioloizou/g1_locomotion, đặc biệt hai file g1_mujoco_sim/src/PD_controller.py và g1_mujoco_sim/src/wbid.py. README của repo mô tả framework là kiến trúc cascade kết hợp Single Rigid Body Dynamics (SRBD) và Whole-Body Inverse Dynamics cho Unitree G1 trong MuJoCo, và có ghi chú rằng implementation này chưa được test trên robot thật.

Mục tiêu không phải là biến bạn thành chuyên gia tối ưu hoá ngay lập tức. Mục tiêu thực dụng hơn: đọc được code, biết mỗi biến đại diện cho gì, hiểu vì sao Kp/Kd và q_init chỉ là tầng thấp nhất, còn robot biết đi cần CoM, Cartesian, DynamicFeasibility, FrictionCone, TorqueLimits và WrenchLimits.

Roadmap series

Series G1 MuJoCo: điều khiển, Foxglove và PlotJuggler gồm 6 bài:

Nếu bạn mới làm quen MuJoCo, đọc thêm Bắt đầu với MuJoCo. Nếu bạn muốn đặt bài này vào bức tranh humanoid rộng hơn, bài Humanoid robot software stack: ROS 2, Isaac và LeRobot là một điểm nối tốt.

q_init: cùng một tư thế, hai cách dùng khác nhau

Trong g1_mujoco_sim/src/config.py, q_init được viết theo convention của Pinocchio/XBot. Vector này bắt đầu bằng floating base 7 phần tử: vị trí (x, y, z) và quaternion (x, y, z, w). Sau đó là 23 joint actuated: 6 joint chân trái, 6 joint chân phải, 1 joint waist yaw, 5 joint tay trái và 5 joint tay phải.

Pose ban đầu rất dễ đọc:

floating base:
  x=0, y=0, z=0.793 - 0.113
  quaternion=(0, 0, 0, 1)

left leg:
  hip_pitch=-0.6, hip_roll=0, hip_yaw=0,
  knee=1.2, ankle_pitch=-0.6, ankle_roll=0

right leg:
  hip_pitch=-0.6, hip_roll=0, hip_yaw=0,
  knee=1.2, ankle_pitch=-0.6, ankle_roll=0

waist:
  waist_yaw=0

arms:
  shoulder/elbow/wrist joints = 0

Đây là một squat-like standing pose: hip pitch âm, knee dương, ankle pitch âm. Nó không phải một policy, cũng không phải một gait. Nó chỉ là cấu hình khởi tạo đủ hợp lý để robot bắt đầu trong MuJoCo mà không phải dựng thẳng đầu gối như cột.

Điểm quan trọng là q_init được hai controller dùng theo hai kiểu khác nhau:

PD coi q_init là đích cố định. WBID coi q_init là điểm xuất phát của model, rồi sau đó reference có thể đến từ MPC, CoM task, base orientation task, contact task và swing foot task. Đây là khác biệt rất lớn. Một bên hỏi "joint hiện tại lệch pose ban đầu bao nhiêu?", bên kia hỏi "robot có thể tạo gia tốc và lực tiếp xúc nào để bám reference mà vẫn không vi phạm động lực học, ma sát và torque?".

PD posture controller thực sự làm gì?

File PD_controller.py rất ngắn. Controller bỏ floating base, chỉ đọc:

q_current = data.qpos[7:]
q_desired = q_init[7:]
dq_current = data.qvel[6:]
dq_desired = np.zeros(dq_current.shape)

Trong MuJoCo, qpos của free-floating humanoid có 7 phần tử base, còn qvel có 6 phần tử base velocity. Vì vậy actuated joint position bắt đầu từ qpos[7:], actuated joint velocity bắt đầu từ qvel[6:]. Đây là chi tiết beginner rất hay nhầm: quaternion base chiếm 4 phần tử position, nhưng angular velocity base chỉ chiếm 3 phần tử velocity.

Sau đó code tạo hai mảng gain 23 phần tử:

Kp[0:6]   = [530, 570, 550, 270, 130, 30]
Kd[0:6]   = [60, 100, 2, 20, 100.5, 5]
Kp[6:12]  = giống chân trái
Kd[6:12]  = giống chân trái
Kp[12]    = 150
Kd[12]    = 10
Kp[13:17] = Kp[18:22] = 20
Kp[17]    = Kp[22] = 11
Kd[13:17] = Kd[18:22] = 5
Kd[17]    = Kd[22] = 0.1

Mapping theo q_init, 12 phần tử đầu là hai chân, phần tử 12 là waist yaw, 10 phần tử cuối là hai tay. Chân có gain lớn vì chúng chống đỡ khối lượng robot. Waist có gain trung bình. Tay có gain nhỏ hơn vì trong bài toán đứng yên, tay không phải nơi tạo lực nền chính.

Công thức torque cuối cùng là:

tau = tau_ff + 1.5 * scale * Kp * (q_desired - q_current) \
    + scale / 3 * Kd * (dq_desired - dq_current)
data.ctrl = tau

Với scale = 1.5, hệ số proportional thực tế là 2.25 * Kp, còn damping là 0.5 * Kd. tau_ff đang bằng 0, nghĩa là controller này không bù trọng lực một cách tường minh, không tính inertia matrix, không tính Coriolis, không tính contact wrench. Nó chỉ đẩy từng joint về góc mong muốn và giảm tốc độ joint.

Nếu robot đang đứng hai chân trên mặt phẳng, posture PD có thể tạo cảm giác "ổn" trong viewer. Nhưng đó là ổn theo nghĩa rất hẹp: robot cố giữ góc joint, còn MuJoCo tự giải tiếp xúc giữa bàn chân và nền. Controller không biết chân nào đang chống, chân nào đang swing, CoM đang lệch khỏi support polygon bao nhiêu, lực ma sát có đủ không, hay torque yêu cầu có vượt limit không. Khi bạn yêu cầu bước đi, PD posture sẽ kéo chân về pose ban đầu, tức là nó chống lại chuyển động swing thay vì tổ chức lực để bước.

Vì sao PD không đủ cho walking?

Walking không phải là chuỗi pose rời rạc. Walking là bài toán động lực học có tiếp xúc. Một robot hai chân phải đồng thời làm các việc sau:

1. Giữ CoM đi theo quỹ đạo hợp lý.
2. Điều khiển orientation của pelvis/torso.
3. Giữ chân stance không trượt trên nền.
4. Đưa chân swing tới landing position.
5. Phân bổ lực nền giữa heel/toe hoặc các contact point.
6. Không yêu cầu lực ma sát vượt cone.
7. Không yêu cầu torque vượt khả năng actuator.
8. Không phá joint limit và velocity limit.

PD posture chỉ nhìn lỗi joint độc lập. Ngay cả khi mọi joint đều gần q_init, robot vẫn có thể ngã nếu CoM nằm sai phía so với chân đang chống. Ngược lại, khi đi bộ, nhiều joint phải cố tình lệch khỏi q_init để tạo bước. Nếu controller cứ kéo về posture cũ, nó sẽ triệt tiêu chính hành động cần thiết để di chuyển.

Một cách nhìn đơn giản:

Posture PD:
  q_desired - q_current
        |
        v
  23 joint torques

WBID:
  desired CoM, base orientation, stance foot, swing foot, MPC forces
        |
        v
  optimize qddot + contact forces
        |
        v
  inverse dynamics torques under contact/friction/torque limits

Vì vậy PD là một baseline tốt để kiểm tra model, joint order, sign convention và actuator path. Nhưng nếu mục tiêu là locomotion, PD posture là chưa đủ. Nó thiếu toàn bộ tầng "robot có được phép làm vậy trong vật lý không?".

WBID tạo bài toán tối ưu như thế nào?

Trong wbid.py, class chính là WholeBodyID. Constructor nhận urdf, dt, q_init và friction_coef=0.8. Nó tạo ModelInterface2 từ XBot, đọc joint limit, velocity limit và effort limit từ URDF, rồi đặt:

self.q = q_init
self.dq = np.zeros(self.model.nv)

Bài toán thật sự được dựng trong WholeBodyID.setupProblem(). Đầu tiên model được update ở q_init. Sau đó code khai báo biến tối ưu:

variables_vec = dict()
variables_vec["qddot"] = self.model.nv

qddot là generalized acceleration của robot. Vì humanoid có floating base, self.model.nv bao gồm cả 6 DOF base velocity space và các joint actuated. Đây không phải chỉ là 23 joint acceleration. WBID phải giải cho gia tốc toàn thân, rồi sau đó inverse dynamics mới cho ra torque actuator.

Tiếp theo là bốn biến lực tiếp xúc, mỗi biến 3 chiều:

line_foot_contact_frames = [
    "left_foot_line_contact_lower",
    "left_foot_line_contact_upper",
    "right_foot_line_contact_lower",
    "right_foot_line_contact_upper",
]

for contact_frame in self.contact_frames:
    variables_vec[contact_frame] = 3

Tên lower/upper trong repo tương ứng hai điểm line-contact trên mỗi bàn chân, thường có thể nghĩ như heel/toe theo hướng bàn chân. Hai contact cho chân trái và hai contact cho chân phải tạo tổng cộng 12 biến lực: mỗi contact có Fx, Fy, Fz. Khi publish /srbd_current, ros_run_simulation.py cũng dùng đúng thứ tự này để gắn force vào message ContactPoint.

Các task trong WBID

Sau khi có biến tối ưu, setupProblem() thêm các task. Đừng đọc chúng như một danh sách API rời rạc. Hãy đọc theo câu hỏi điều khiển:

Trong stack cụ thể, CoM có weight lớn (3.*self.com). Posture chỉ áp dụng lên subset index [18, 19, ..., 28], tức không phải kéo toàn bộ robot về q_init như PD. Base task chỉ lấy orientation part bằng self.base % [3, 4, 5]. Contact task và swing task cùng tồn tại, nhưng được bật/tắt theo gait phase trong ros_run_simulation.py.

Đây là điểm khác biệt tinh tế: WBID vẫn có posture task, nhưng posture chỉ là một ưu tiên mềm trong một stack có nhiều mục tiêu và ràng buộc. PD posture biến posture thành toàn bộ controller. Với humanoid walking, posture nên là "giữ dáng phụ khi còn dư tự do", không phải luật duy nhất.

Reference từ MPC đi vào WBID ra sao?

Khi có message /mpc_solution, ros_run_simulation.py unpack thành:

x_opt[i, 0:3]   = roll, pitch, yaw
x_opt[i, 3:6]   = CoM position
x_opt[i, 6:9]   = angular velocity
x_opt[i, 9:12]  = linear velocity
x_opt[i, 12]    = gravity
u_opt0          = 12 lực contact, 3 lực cho mỗi contact frame
contact_states  = active/inactive cho bốn contact

Trong mỗi sim_step, code update model, xử lý contact phase, gọi:

WBID.stack.update()
WBID.setReference(self.sim_time, self.x_opt[1, :], self.u_opt0, foot_positions_curr)
WBID.solveQP()
tau = WBID.getInverseDynamics()
self.data.ctrl = tau[6:]

setReference() dùng x_opt1[0:3] để set orientation của pelvis, dùng x_opt1[3:6] và x_opt1[9:12] cho CoM position/velocity, rồi tính CoM acceleration reference từ tổng contact forces u_opt0 chia cho mass cộng gravity. Đồng thời, mỗi wrench_task nhận reference lực 3D tương ứng từ MPC.

Nói cách khác, MPC không ra lệnh trực tiếp "joint hip pitch bằng bao nhiêu". MPC đưa ra kế hoạch centroidal: torso orientation, CoM, vận tốc và lực nền. WBID chuyển kế hoạch đó thành gia tốc toàn thân và torque actuator trong model đầy đủ.

Ràng buộc: phần PD không có

Phần quan trọng nhất của wbid.py không chỉ là task mà là constraint. Sau khi tạo stack, code thêm:

DynamicFeasibility
JointLimits
VelocityLimits
TorqueLimits
FrictionCone
WrenchLimits

DynamicFeasibility buộc nghiệm qddot và các contact force phải thoả floating-base dynamics. Với robot có base không được actuator trực tiếp, đây là sống còn. Bạn không thể tùy ý yêu cầu pelvis bay sang phải nếu lực chân và torque joint không tạo được gia tốc đó.

FrictionCone dùng hệ số ma sát friction_coef=0.8. Nếu lực tiếp tuyến quá lớn so với lực pháp tuyến, bàn chân sẽ trượt trong vật lý. PD không biết điều này; nó chỉ tạo torque joint. WBID đưa lực tiếp xúc vào biến tối ưu, nên có thể ràng buộc lực đó nằm trong cone.

TorqueLimits dùng effort limit từ URDF. Đây là lớp bảo vệ khỏi nghiệm "đẹp trên giấy nhưng actuator không thể tạo". WrenchLimits đặt giới hạn từng contact: mặc định [-1000, -1000, 10] đến [1000, 1000, 1000], tức normal force có lower bound dương khi contact đang chống. Khi một chân swing, ros_run_simulation.py đặt wrench limits cho hai contact của chân swing về zero để không cho chân đang nhấc vẫn tạo lực nền.

Bảng đọc nhanh:

Tài liệu MuJoCo về computation và inverse dynamics cũng nhấn mạnh động lực học tiếp xúc không thể được xem như một bài toán joint-only đơn giản: inverse dynamics trong hệ có contact phải xử lý lực tiếp xúc và ràng buộc. Chính vì vậy một humanoid walking stack nghiêm túc thường phải theo dõi cả gia tốc, lực, ma sát và giới hạn actuator, không chỉ mỗi lỗi góc joint.

Từ nghiệm QP đến data.ctrl

Sau solveQP(), WBID lấy nghiệm:

self.ddq = self.variables.getVariable("qddot").getValue(self.x)
self.contact_forces.append(
    self.variables.getVariable(contact_frame).getValue(self.x)
)

Sau đó getInverseDynamics() set acceleration cho model, gọi computeInverseDynamics(), rồi trừ phần contact wrench qua Jacobian:

tau = self.model.computeInverseDynamics()
for contact_frame:
    Jc = self.model.getJacobian(contact_frame)
    tau = tau - Jc[:3, :].T @ contact_force
return tau

Trong ros_run_simulation.py, torque gửi vào MuJoCo là tau[6:], tức loại bỏ 6 DOF floating base:

self.data.ctrl = tau[6:]

Điều này giống PD ở điểm cuối cùng: cả hai đều ghi vào data.ctrl. Nhưng đường đi khác hoàn toàn. PD đi thẳng từ lỗi joint sang torque. WBID đi từ MPC/reference sang QP, qua constraint, qua inverse dynamics, rồi mới ra torque actuator.

Checklist debug khi robot ngã

Khi robot ngã trong bài toán này, đừng sửa gain mù. Hãy tách lỗi theo tầng:

Đây là lúc quay lại bài 3 PlotJuggler. Hãy vẽ /mpc_solution, /srbd_current, /feet_ref_pos và /wbid_statistics/full/statistics[0]/value. Nếu MPC đã xấu, sửa MPC. Nếu MPC đẹp nhưng WBID không bám, xem contact, task weight và constraint. Nếu WBID solve chậm, đừng chỉ tăng gain; hãy kiểm tra reference có liên tục không và bài toán có feasible không.

Kết luận

PD_controller.py là một baseline tốt vì nó ngắn, dễ đọc và kiểm tra được đường actuator từ Python tới MuJoCo. Nó dùng q_init[7:], Kp/Kd theo nhóm joint, velocity desired bằng zero và ghi torque trực tiếp vào data.ctrl. Với mục tiêu giữ dáng, cách này đủ để bắt đầu.

wbid.py là controller cho bài toán khác: walking có tiếp xúc. Nó tối ưu qddot và bốn contact force 3D, bám CoM/base/swing/contact reference, rồi ràng buộc nghiệm bằng dynamic feasibility, friction cone, torque limit và wrench limit. Đó là lý do WBID phức tạp hơn nhiều so với PD, nhưng cũng là lý do nó có cơ hội tạo chuyển động đi bộ hợp lý trong MuJoCo.

Ở bài 5, ta sẽ đi lên upper body: khi chân và CoM đã có logic điều khiển, tay, waist và IK nên được nối vào stack thế nào để không phá locomotion.

Bài viết liên quan

• Debug MPC/WBID G1 bằng PlotJuggler
• Upper-body IK cho G1
• Checklist sim-to-real