Humanoid control cơ bản: IK đến task-space control

Tại sao điều khiển Humanoid khó?

Humanoid robot có hàng chục bậc tự do (DOF), phải giữ thăng bằng trên 2 chân trong khi thực hiện nhiệm vụ bằng tay. So sánh với robot arm cố định trên bàn (6 DOF, không cần balance), humanoid phức tạp hơn nhiều bậc.

Bài này sẽ đi từ cơ bản đến nâng cao: Inverse Kinematics (IK) -> Jacobian -> Operational Space Control -> Balance control (ZMP, CoM) -> Task Prioritization. Mỗi phần đều có code Python để bạn thực hành.

Inverse Kinematics (IK) -- Nền tảng đầu tiên

IK trả lời câu hỏi: "Cho vị trí mong muốn của end-effector (tay, chân), tìm góc các khớp tương ứng."

Forward Kinematics (FK)

FK là quá trình ngược: cho góc các khớp, tính vị trí end-effector. Với chuỗi khớp, ta dùng phép biến đổi thuần nhất (homogeneous transformation):

T_0_n = T_0_1 * T_1_2 * ... * T_(n-1)_n

Mỗi T_i_(i+1) là ma trận 4x4 biểu diễn rotation và translation giữa 2 khớp liên tiếp, thường dùng Denavit-Hartenberg (DH) parameters.

IK bằng Jacobian (Numerical)

Với humanoid robot có 30+ DOF, analytical IK rất khó (hoặc không thể). Phương pháp số (numerical) dùng Jacobian matrix:

import numpy as np
import mujoco

def compute_jacobian(model, data, body_id):
    """Tính Jacobian cho 1 body trong MuJoCo."""
    jacp = np.zeros((3, model.nv))  # position Jacobian
    jacr = np.zeros((3, model.nv))  # rotation Jacobian
    mujoco.mj_jacBody(model, data, jacp, jacr, body_id)
    return np.vstack([jacp, jacr])  # 6 x nv

def ik_step(model, data, body_id, target_pos, target_quat, step_size=0.5):
    """1 bước IK dùng damped least squares."""
    # Tính vị trí hiện tại
    current_pos = data.xpos[body_id].copy()
    current_quat = data.xquat[body_id].copy()
    
    # Tính sai số vị trí
    pos_error = target_pos - current_pos
    
    # Tính sai số orientation (dùng quaternion)
    quat_error = np.zeros(3)
    mujoco.mju_subQuat(quat_error, target_quat, current_quat)
    
    # Gộp lại thành 6D error
    error = np.concatenate([pos_error, quat_error])
    
    # Tính Jacobian
    J = compute_jacobian(model, data, body_id)
    
    # Damped least squares (Levenberg-Marquardt)
    damping = 1e-4
    JtJ = J.T @ J + damping * np.eye(model.nv)
    delta_q = np.linalg.solve(JtJ, J.T @ error)
    
    # Cập nhật joint positions
    data.qpos[:model.nv] += step_size * delta_q
    mujoco.mj_forward(model, data)
    
    return np.linalg.norm(error)

# Chạy IK loop
for i in range(100):
    err = ik_step(model, data, hand_body_id, target_pos, target_quat)
    if err < 1e-3:
        print(f"IK converged after {i+1} iterations")
        break

Damped Least Squares (hay Levenberg-Marquardt) giải quyết vấn đề singularity -- khi Jacobian mất rank (robot ở biên của workspace), nghịch đảo Jacobian không ổn định. Thêm damping lambda * I giúp ổn định nhưng giảm độ chính xác.

IK Libraries cho Humanoid

MuJoCo built-in: mujoco.mj_jacBody() + custom solver
Pink: IK library cho Python, hỗ trợ MuJoCo và Pinocchio
Pinocchio: Rigid body dynamics library, IK + dynamics
Drake: Toolbox của MIT/TRI, mạnh về optimization-based IK

Operational Space Control

IK chỉ tính vị trí mong muốn -- nhưng không điều khiển lực. Trong thực tế, humanoid cần điều khiển cả vị trí lẫn lực -- đây là Operational Space Control (Khatib, 1987).

Ý tưởng chính

Thay vì điều khiển trong joint space (q, dq, tau), ta điều khiển trong task space (x, dx, F):

F = Lambda * x_ddot_desired + mu + p

Trong đó:

Lambda = (J * M^(-1) * J^T)^(-1) -- task-space inertia matrix
mu -- Coriolis/centrifugal forces trong task space
p -- gravity forces trong task space
x_ddot_desired -- gia tốc mong muốn trong task space

Chuyển đổi ngược về joint torques:

tau = J^T * F + N^T * tau_null

Trong đó N = I - J^T * Lambda * J * M^(-1) là null-space projector -- cho phép thực hiện nhiệm vụ phụ (ví dụ giữ thăng bằng) mà không ảnh hưởng nhiệm vụ chính.

Code Operational Space Control

import numpy as np
import mujoco

class OperationalSpaceController:
    def __init__(self, model, data, body_id):
        self.model = model
        self.data = data
        self.body_id = body_id
        self.nv = model.nv
        
    def compute_task_space_control(self, x_des, dx_des, ddx_des, Kp, Kd):
        """Tính joint torques từ task-space command."""
        m = self.model
        d = self.data
        
        # Mass matrix
        M = np.zeros((self.nv, self.nv))
        mujoco.mj_fullM(m, M, d.qM)
        
        # Jacobian
        J = compute_jacobian(m, d, self.body_id)[:3]  # Chỉ position
        
        # Task-space inertia
        M_inv = np.linalg.inv(M)
        Lambda_inv = J @ M_inv @ J.T
        Lambda = np.linalg.inv(Lambda_inv)
        
        # Current task-space state
        x = d.xpos[self.body_id].copy()
        dx = J @ d.qvel
        
        # PD control trong task space
        ddx_cmd = ddx_des + Kp * (x_des - x) + Kd * (dx_des - dx)
        
        # Task-space force
        F = Lambda @ ddx_cmd
        
        # Coriolis compensation
        bias = d.qfrc_bias.copy()  # C(q,dq)*dq + g(q)
        mu_p = J @ M_inv @ bias
        F += mu_p
        
        # Map về joint torques
        tau = J.T @ F
        
        # Null-space: giữ posture mặc định
        N = np.eye(self.nv) - J.T @ Lambda @ J @ M_inv
        q_default = np.zeros(self.nv)
        tau_null = 10.0 * (q_default - d.qpos[:self.nv]) - 2.0 * d.qvel
        tau += N.T @ tau_null
        
        return tau

Balance Control -- ZMP và CoM

Humanoid cần giữ thăng bằng trong khi thực hiện nhiệm vụ. Hai khái niệm quan trọng:

Center of Mass (CoM)

CoM là điểm trọng tâm khối lượng của toàn bộ robot. Để robot không ngã, hình chiếu CoM xuống mặt đất phải nằm trong support polygon (vùng tạo bởi các điểm tiếp xúc chân với mặt đất).

Zero Moment Point (ZMP)

ZMP là điểm trên mặt đất mà tại đó tổng moment của tất cả lực (trọng lực + quán tính) bằng 0. Điều kiện cân bằng:

ZMP nằm trong support polygon

Khi ZMP tiếp cận biên của support polygon, robot sắp ngã. Controller cần điều chỉnh để kéo ZMP về trung tâm.

Code ZMP tính toán

def compute_zmp(model, data):
    """Tính ZMP từ MuJoCo simulation data."""
    # Tổng khối lượng
    total_mass = sum(model.body_mass)
    
    # CoM position và acceleration
    com_pos = np.zeros(3)
    com_acc = np.zeros(3)
    
    for i in range(model.nbody):
        m_i = model.body_mass[i]
        com_pos += m_i * data.xipos[i]
        # Acceleration từ lực tác động
        com_acc += data.xfrc_applied[i, :3]  # Simplified
    
    com_pos /= total_mass
    com_acc /= total_mass
    
    g = 9.81
    # ZMP formula
    zmp_x = com_pos[0] - com_pos[2] * com_acc[0] / (g + com_acc[2])
    zmp_y = com_pos[1] - com_pos[2] * com_acc[1] / (g + com_acc[2])
    
    return np.array([zmp_x, zmp_y])

def compute_support_polygon(contact_positions):
    """Tính support polygon từ các điểm tiếp xúc."""
    from scipy.spatial import ConvexHull
    if len(contact_positions) < 3:
        return contact_positions
    points_2d = contact_positions[:, :2]  # Chỉ lấy x, y
    hull = ConvexHull(points_2d)
    return points_2d[hull.vertices]

Linear Inverted Pendulum Model (LIPM)

Để đơn giản hóa, humanoid thường được mô hình hóa như con lắc ngược tuyến tính (LIPM):

ddot_x_com = (g / z_com) * (x_com - x_zmp)

Từ đây, ta thiết kế controller để điều khiển x_zmp sao cho x_com đi theo quỹ đạo mong muốn. Đây là nền tảng của ZMP-based walking -- phương pháp đi bộ cổ điển nhất cho humanoid.

Task Prioritization -- Làm nhiều việc cùng lúc

Humanoid cần làm nhiều việc đồng thời: giữ thăng bằng (ưu tiên cao nhất), di chuyển (ưu tiên trung bình), với tay cầm vật (ưu tiên thấp hơn). Task prioritization giải quyết vấn đề này.

Null-Space Projection

Ý tưởng: task ưu tiên thấp được chiếu (project) vào null space của task ưu tiên cao -- nghĩa là task thấp chỉ được thực hiện khi không ảnh hưởng task cao.

def prioritized_control(tasks, model, data):
    """
    tasks = [(J1, F1), (J2, F2), ...] theo thứ tự ưu tiên giảm dần.
    """
    nv = model.nv
    tau = np.zeros(nv)
    N = np.eye(nv)  # Null-space projector
    
    for J_i, F_i in tasks:
        # Project task vào null space của các task trước
        J_bar = J_i @ N
        
        # Pseudoinverse
        J_bar_pinv = np.linalg.pinv(J_bar)
        
        # Torque cho task này
        tau += N @ J_bar_pinv @ F_i
        
        # Cập nhật null space
        N = N @ (np.eye(nv) - J_bar_pinv @ J_bar)
    
    return tau

# Sử dụng:
# Task 1 (cao nhất): Giữ thăng bằng (CoM control)
# Task 2: Di chuyển chân
# Task 3: Với tay cầm vật
# Task 4 (thấp nhất): Giữ posture mặc định
tasks = [
    (J_com, F_balance),
    (J_feet, F_locomotion),
    (J_hand, F_manipulation),
    (J_posture, F_posture)
]
tau = prioritized_control(tasks, model, data)

Whole-Body QP Controller

Trong thực tế, nhiều hệ thống hiện đại dùng Quadratic Programming (QP) thay vì null-space projection:

minimize: ||J_task * ddq - ddx_desired||^2 + w * ||tau||^2
subject to:
    M * ddq + C * dq + g = S * tau + J_c^T * f_c   (dynamics)
    f_c trong friction cone                           (no slip)
    tau_min <= tau <= tau_max                          (actuator limits)
    ZMP trong support polygon                         (balance)

QP cho phép xử lý constraints (giới hạn torque, friction cone, balance) một cách tự nhiên -- điều mà null-space projection không làm được.

Libraries phổ biến: qpOASES, OSQP, Pinocchio + ProxQP.

Tổng kết pipeline điều khiển

Task Planner (cầm vật, di chuyển, ...)
    |
    v
Task-Space Controller (Operational Space / QP)
    |
    v
Balance Controller (ZMP / CoM tracking)
    |
    v
Whole-Body IK / ID (Jacobian, dynamics)
    |
    v
Joint Torque Commands -> Actuators

Mỗi lớp giải quyết một vấn đề khác nhau, từ high-level planning đến low-level torque control. Trong bài tiếp theo, chúng ta sẽ đi sâu vào Model Predictive Control (MPC) -- phương pháp mạnh hơn để điều khiển toàn thân humanoid trong thời gian thực.