双臂操作（Bimanual Manipulation）是人形机器人区别于传统工业机械臂的核心能力之一。它使机器人能够操纵尺寸或质量超出单臂能力的物体，并执行需要协同的复杂灵巧任务（如装配、开门）。当双臂协同与机体的移动（Locomotion）相结合时，便构成了全身运动操作（Whole-Body Loco-Manipulation），这是人形机体在非结构化环境中执行物理交互的根本。

本章旨在深入推导双臂闭环系统的运动学与动力学原理，重点解析任务分解中的核心问题——力分配（Force Distribution）；并进一步将此模型扩展至全身动力学，推导大物体搬运过程中，操纵、运动与平衡（ZMP）之间强耦合关系的控制原理。

---

16.1 双臂闭环系统的运动学与静力学

当两只手臂（Left, L; Right, R）同时刚性地抓握一个物体（Object, O）时，系统形成了一个并联的闭环运动链。

16.1.1 闭环运动学约束

16.1.2 静力学对偶与力分配

---

16.2 任务分解与全身优化控制

16.2.1 任务空间动力学

---

16.3 大物体搬运与全身平衡

当人形机体搬运大物体（如桌子、箱子）并行走时，问题升级为全身运动操作（Whole-Body Loco-Manipulation）。此时，操纵臂、腿部、躯干和物体被耦合为一个统一的动力学系统。

16.3.1 统一动力学与 ZMP 稳定性

16.3.2 操纵-运动的强耦合

16.3.3 握持与重新定位策略（全身控制器）

16.4.1 抓握矩阵的构建

import numpy as np
import scipy.linalg

# 辅助函数：将 3D 向量转换为 3x3 反对称矩阵
def skew(v):
    """
    将一个 3D 向量 v = [x, y, z] 转换为其 3x3 反对称矩阵（skew-symmetric matrix）。
    | 0 -z  y|
    | z  0 -x|
    |-y  x  0|
    """
    v = v.flatten()
    if v.shape[0] != 3:
        raise ValueError("输入向量必须是 3D")
    return np.array([
        [0, -v[2], v[1]],
        [v[2], 0, -v[0]],
        [-v[1], v[0], 0]
    ])

def build_grasp_matrix_transpose(r_L_o, r_R_o):
    """
    构建 6x12 的静力学抓握矩阵 G.T
    
    参数:
    r_L_o (np.array[3,1] or [3,]): 从物体质心 O 到左手 L 的位置向量 (在世界系中表示)
    r_R_o (np.array[3,1] or [3,]): 从物体质心 O 到右手 R 的位置向量 (在世界系中表示)
    
    返回:
    G_T (np.array[6,12]): 静力学抓握矩阵
    """
    I = np.eye(3)
    Z = np.zeros((3, 3))
    
    # 构建 G_L.T
    r_L_skew = skew(r_L_o)
    # G_L.T = | I  0 |
    #         |-rL  I |
    G_L_T = np.block([
        [I, Z],
        [-r_L_skew, I]
    ])
    
    # 构建 G_R.T
    r_R_skew = skew(r_R_o)
    # G_R.T = | I  0 |
    #         |-rR  I |
    G_R_T = np.block([
        [I, Z],
        [-r_R_skew, I]
    ])
    
    # G.T = [G_L.T, G_R.T]
    G_T = np.hstack([G_L_T, G_R_T])
    
    return G_T

代码实现

#!/usr/bin/env python
# -*- coding: utf-8 -*-

"""
第16章 代码实现：双臂协同与全身控制
=====================================

本文件汇总了第16章中用于演示双臂力分配、
机器人动力学计算和全身控制器（WBC）QP构建的核心代码。

请确保已安装所需库:
pip install numpy scipy pinocchio cvxpy

"""

import numpy as np
import scipy.linalg
import pinocchio as pin
import cvxpy as cp

# ==============================================================================
# 16.4 双臂力分配的数值实现 (NumPy)
# ==============================================================================

def skew(v):
    """
    将一个 3D 向量 v = [x, y, z] 转换为其 3x3 反对称矩阵（skew-symmetric matrix）。
    | 0 -z  y|
    | z  0 -x|
    |-y  x  0|
    """
    v = v.flatten()
    if v.shape[0] != 3:
        raise ValueError("输入向量必须是 3D")
    return np.array([
        [0, -v[2], v[1]],
        [v[2], 0, -v[0]],
        [-v[1], v[0], 0]
    ])

def build_grasp_matrix_transpose(r_L_o, r_R_o):
    """
    构建 6x12 的静力学抓握矩阵 G.T (G_T)
    
    参数:
    r_L_o (np.array[3,1] or [3,]): 从物体质心 O 到左手 L 的位置向量
    r_R_o (np.array[3,1] or [3,]): 从物体质心 O 到右手 R 的位置向量
    
    返回:
    G_T (np.array[6,12]): 静力学抓握矩阵
    """
    I = np.eye(3)
    Z = np.zeros((3, 3))
    
    # 构建 G_L.T
    r_L_skew = skew(r_L_o)
    # G_L.T = | I  0 |
    #         |-rL  I |
    G_L_T = np.block([
        [I, Z],
        [-r_L_skew, I]
    ])
    
    # 构建 G_R.T
    r_R_skew = skew(r_R_o)
    G_R_T = np.block([
        [I, Z],
        [-r_R_skew, I]
    ])
    
    # G.T = [G_L.T, G_R.T]
    G_T = np.hstack([G_L_T, G_R_T])
    
    return G_T

def calculate_force_distribution(G_T, W_des, lambda_int):
    """
    计算双臂力分配。
    
    参数:
    G_T (np.array[6,12]): 静力学抓握矩阵
    W_des (np.array[6,1]): 期望施加于物体的净力旋量 (v_des, w_des)
    lambda_int (np.array[6,1]): 期望施加的 6-DoF 内力向量
    
    返回:
    F_L (np.array[6,1]): 左手施加的力旋量
    F_R (np.array[6,1]): 右手施加的力旋量
    F_p (np.array[12,1]): 外部力（特解）
    F_h (np.array[12,1]): 内部力（齐次解）
    """
    
    # 确保输入是 (N, 1) 的列向量
    W_des = W_des.reshape(-1, 1)
    lambda_int = lambda_int.reshape(-1, 1)

    # 1. 计算特解 (F_p) - 外部力
    G_T_pseudo_inv = np.linalg.pinv(G_T) 
    F_p = G_T_pseudo_inv @ W_des
    
    # 2. 计算齐次解 (F_h) - 内力
    # N_int 的维度是 (12, 6)
    N_int = scipy.linalg.null_space(G_T)
    
    # 确保 N_int 的维度正确
    if N_int.shape[1] != 6:
        print(f"警告: 零空间维度不是 6, 而是 {N_int.shape[1]}。抓握矩阵可能存在奇异。")
        # 即使奇异，也尝试找到最接近的解
        if N_int.shape[1] < 6:
            # 维度不足，无法施加 6D 内力
            lambda_int_proj = lambda_int[:N_int.shape[1]]
            N_int_proj = N_int
        else:
            # 维度过多（不可能）
            lambda_int_proj = lambda_int
            N_int_proj = N_int[:, :6]
        F_h = N_int_proj @ lambda_int_proj
    else:
        F_h = N_int @ lambda_int
    
    # 3. 计算总力
    F_hand = F_p + F_h
    
    # 提取 F_L 和 F_R
    F_L = F_hand[0:6]
    F_R = F_hand[6:12]
    
    return F_L, F_R, F_p, F_h, N_int

def run_force_distribution_example():
    print("-" * 60)
    print("运行 16.4 力分配示例...")
    print("-" * 60)
    
    r_L_o = np.array([0.5, 0, 0])
    r_R_o = np.array([-0.5, 0, 0])
    G_T = build_grasp_matrix_transpose(r_L_o, r_R_o)

    # 任务 1: 仅向上提起 (Z轴正向力 10N)，无内力
    W_des_lift = np.array([0, 0, 10, 0, 0, 0])
    lambda_int_zero = np.zeros(6)
    F_L, F_R, _, _, _ = calculate_force_distribution(G_T, W_des_lift, lambda_int_zero)
    print("任务1: 向上提 10N (无内力)")
    print(f"  F_L (左手力): {F_L.flatten()}")
    print(f"  F_R (右手力): {F_R.flatten()}")
    # 预期结果: F_L[2] = 5N, F_R[2] = 5N (力被平均分配)
    
    # 任务 2: 仅施加内力 (例如，lambda_int的第一个分量)
    W_des_zero = np.zeros(6)
    lambda_int_squeeze = np.array([10.0, 0, 0, 0, 0, 0]) # 假设10个单位的内力
    F_L_sq, F_R_sq, _, F_h, _ = calculate_force_distribution(G_T, W_des_zero, lambda_int_squeeze)
    W_check = G_T @ (F_L_sq + F_R_sq) # 这不正确，G_T @ F_hand
    W_check = G_T @ np.vstack([F_L_sq, F_R_sq])
    print("\n任务2: 施加 10 单位内力 (无净力)")
    print(f"  F_L (左手力): {F_L_sq.flatten()}")
    print(f"  F_R (右手力): {F_R_sq.flatten()}")
    print(f"  产生的净力 W_des (应为~0): {W_check.flatten()}")
    print("-" * 60)


# ==============================================================================
# 16.5 机器人动力学模型的构建 (Pinocchio)
# ==============================================================================

def run_pinocchio_dynamics_example():
    print("\n" + "-" * 60)
    print("运行 16.5 Pinocchio 动力学计算示例...")
    print("-" * 60)
    
    try:
        # 1. 加载 Pinocchio 自带的示例模型 (Talos 双臂)
        model = pin.buildSampleModel('talos') 
        # model = pin.buildSampleModel('talos_arm') # 仅手臂
        
        # 2. 创建数据结构
        data = model.createData()

        # 3. 定义机器人状态 (随机)
        q = pin.randomConfiguration(model)
        v = np.random.rand(model.nv)
        a = np.zeros(model.nv) # 假设的广义加速度
        
        print(f"模型加载成功: {model.name}")
        print(f"  广义坐标 q 维度: {model.nq}")
        print(f"  广义速度 v 维度: {model.nv}")

        # --- 4. 计算动力学量 (WBC 的 "输入参数") ---

        # 4.1. M(q): 质量矩阵 (CRBA)
        pin.crba(model, data, q)
        M = data.M
        print(f"\n计算 M(q): 维度 {M.shape}")

        # 4.2. nle(q, v): C(q,v)v + G(q) (RNEA)
        pin.rnea(model, data, q, v, a) # a=0 时, tau = nle
        nle = data.tau 
        print(f"计算 nle(q,v): 维度 {nle.shape}")

        # 4.3. J(q) 和 dJ/dt * v: 雅可比与偏加速度
        # 必须先运行完整的正向运动学 (q, v)
        pin.forwardKinematics(model, data, q, v)
        pin.computeJointJacobians(model, data, q)

        # 获取特定帧（例如，手爪）
        # 注意: 'talos' 示例中的帧ID可能不同
        LEFT_HAND_FRAME = "arm_left_7_link"
        RIGHT_HAND_FRAME = "arm_right_7_link"
        LEFT_FOOT_FRAME = "left_sole_link"
        
        frame_id_L = model.getFrameId(LEFT_HAND_FRAME)
        frame_id_R = model.getFrameId(RIGHT_HAND_FRAME)
        frame_id_foot = model.getFrameId(LEFT_FOOT_FRAME)

        # 4.3.1 雅可比
        J_L = pin.getFrameJacobian(model, data, frame_id_L, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
        J_R = pin.getFrameJacobian(model, data, frame_id_R, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
        J_foot = pin.getFrameJacobian(model, data, frame_id_foot, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
        
        print(f"\n计算 J_L (左手雅可比): 维度 {J_L.shape}")
        
        J_arms = np.vstack([J_L, J_R])

        # 4.3.2 偏加速度 (Bias Acceleration, dJ/dt * v)
        # getFrameAcceleration 返回的是一个 pin.Motion 对象
        acc_L_bias = pin.getFrameAcceleration(model, data, frame_id_L, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED).np
        acc_R_bias = pin.getFrameAcceleration(model, data, frame_id_R, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED).np
        acc_foot_bias = pin.getFrameAcceleration(model, data, frame_id_foot, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED).np

        acc_arms_bias = np.vstack([acc_L_bias, acc_R_bias])
        print(f"计算 acc_L_bias (dJ_L/dt * v): 维度 {acc_L_bias.shape}")
        
        # 4.4 质心 (CoM) 雅可比
        pin.computeCentroidalMomentum(model, data)
        J_com = data.Jcom
        # 质心偏加速度 (dJ_com/dt * v)
        # 需在 rnea 之后调用
        pin.rnea(model, data, q, v, a) # a=0
        acc_com_bias = pin.getCentroidalAcceleration(model, data) 
        
        print(f"计算 J_com (质心雅可比): 维度 {J_com.shape}")
        print("-" * 60)
        
        # 返回 WBC 所需的所有参数
        return model, data, q, v, nle, M, J_arms, acc_arms_bias, J_foot, acc_foot_bias, J_com, acc_com_bias.np

    except ImportError:
        print("Pinocchio 库未安装。请通过 'pip install pinocchio' 或 'conda install -c conda-forge pinocchio' 安装。")
        return [None] * 12
    except Exception as e:
        print(f"Pinocchio 运行出错: {e}")
        print("请确保 Pinocchio 示例模型 'talos' 可用。")
        return [None] * 12


# ==============================================================================
# 16.6 全身控制器 (WBC) 的 QP 构架 (CVXPY)
# ==============================================================================

def run_wbc_qp_example(wbc_inputs, force_dist_inputs):
    print("\n" + "-" * 60)
    print("运行 16.6 WBC QP 构建示例 (CVXPY)...")
    print("-" * 60)
    
    (model, data, q, v, nle, M, J_arms, acc_arms_bias, J_foot, acc_foot_bias, J_com, acc_com_bias) = wbc_inputs
    (G_T, N_int, F_p) = force_dist_inputs

    if model is None:
        print("因 Pinocchio 加载失败，跳过 WBC 示例。")
        return

    try:
        nv = model.nv # 广义速度的维度
        
        # --- 1. 定义 QP 决策变量 ---
        q_ddot = cp.Variable(nv, name="q_ddot")
        lambda_int = cp.Variable(6, name="lambda_int")
        
        print(f"构建 QP 问题：")
        print(f"  变量 q_ddot: {q_ddot.shape}")
        print(f"  变量 lambda_int: {lambda_int.shape}")

        # --- 2. 准备任务和约束的“常量” ---
        
        # 2.1 任务目标 (Desired values)
        # 假设：保持当前姿态 (PD)
        q_ref = pin.neutral(model) # 假设目标是中性姿态
        Kp = 10.0
        Kd = 0.5
        acc_posture_des = Kp * (q_ref - q) - Kd * v
        # 假设：ZMP 目标 (保持静止)
        acc_com_des = np.zeros(6) 
        # 假设：内力目标
        lambda_int_des = np.zeros(6)
        
        # 2.2 约束限制
        tau_max = model.effortLimit
        tau_min = -model.effortLimit

        # 2.3 抓握/摩擦锥 (假设)
        # 抓握 GWS (12x12, 12x1)
        A_grasp = np.eye(12) 
        b_grasp = np.ones(12) * 100.0 # 假设最大抓握力 100N/Nm
        # 地面摩擦锥 (假设)
        # ... (为简化，此处省略 F_foot 的 QP 构建)
        
        # --- 3. 构建约束 (Constraints) ---
        constraints = []
        print("添加约束...")

        # 3.1 闭环运动学约束 (J*q_ddot = -J_dot*v)
        # 假设物体固定 (acc_O_des = 0, acc_G_bias = 0)
        A_kin = J_arms
        b_kin = -acc_arms_bias.flatten()
        constraints.append( A_kin @ q_ddot == b_kin )
        print("  [C1] 闭环运动学 (J_arms q_ddot = -J_dot_v)")

        # 3.2 支撑脚约束
        A_foot = J_foot
        b_foot = -acc_foot_bias.flatten()
        constraints.append( A_foot @ q_ddot == b_foot )
        print("  [C2] 支撑脚 (J_foot q_ddot = -J_dot_v)")

        # 3.3 动力学与力矩限制
        # tau = M*q_ddot + nle - J_arms.T * (F_p + N_int * lambda_int)
        # 浮动基座没有力矩 (前 6 个自由度)
        tau = M @ q_ddot + nle - J_arms.T @ (F_p.flatten() + N_int @ lambda_int)
        
        # 仅约束有关节 (actuated) 的自由度 (跳过前6个浮动基座)
        ACTUATED_JOINTS_START_IDX = 6 
        tau_actuated = tau[ACTUATED_JOINTS_START_IDX:]
        tau_max_actuated = tau_max[ACTUATED_JOINTS_START_IDX:]
        tau_min_actuated = tau_min[ACTUATED_JOINTS_START_IDX:]

        constraints.append( tau_actuated <= tau_max_actuated )
        constraints.append( tau_actuated >= tau_min_actuated )
        print(f"  [C3] 力矩限制 (tau_min <= tau <= tau_max) for {len(tau_actuated)} joints")

        # 3.4 抓握摩擦锥约束 (GWS)
        # (A_grasp @ N_int) * lambda_int <= b_grasp - A_grasp @ F_p
        A_g = A_grasp @ N_int
        b_g = b_grasp - (A_grasp @ F_p.flatten())
        constraints.append( A_g @ lambda_int <= b_g )
        print("  [C4] 抓握约束 (GWS)")

        # --- 4. 构建目标函数 (Objective) ---
        print("构建目标函数...")
        cost = 0.0

        # T1: ZMP / 质心加速度 (最高权重)
        weight_zmp = 1.0 # 权重应仔细调整
        acc_com = J_com @ q_ddot + acc_com_bias.flatten()
        cost += weight_zmp * cp.sum_squares(acc_com[0:3] - acc_com_des.flatten()[0:3]) # 仅线性部分

        # T2: 姿态/关节空间正则化
        weight_posture = 0.1
        cost += weight_posture * cp.sum_squares(q_ddot[ACTUATED_JOINTS_START_IDX:] - acc_posture_des[ACTUATED_JOINTS_START_IDX:])

        # T3: 内力跟踪
        weight_lambda = 0.01
        cost += weight_lambda * cp.sum_squares(lambda_int - lambda_int_des)

        # T4: 最小化关节力矩 (正则化)
        weight_tau = 0.0001
        cost += weight_tau * cp.sum_squares(tau_actuated)

        # --- 5. 求解 QP ---
        objective = cp.Minimize(cost)
        problem = cp.Problem(objective, constraints)
        
        print("\n开始求解 QP...")
        # OSQP 是一个高效的求解器
        problem.solve(solver='OSQP', verbose=False, warm_start=True)

        if problem.status == 'optimal':
            print("QP 求解成功!")
            q_ddot_sol = q_ddot.value
            lambda_int_sol = lambda_int.value
            print(f"  q_ddot (norm): {np.linalg.norm(q_ddot_sol)}")
            print(f"  lambda_int: {lambda_int_sol}")
        elif problem.status == 'infeasible':
            print("QP 求解失败: 问题无解 (Infeasible)!")
        elif problem.status == 'unbounded':
            print("QP 求解失败: 问题无界 (Unbounded)!")
        else:
            print(f"QP 求解失败: {problem.status}")
        
        print("-" * 60)

    except ImportError:
        print("CVXPY 库未安装。请通过 'pip install cvxpy' 安装。")
    except Exception as e:
        print(f"CVXPY 运行出错: {e}")


# ==============================================================================
# 主执行函数
# ==============================================================================

if __name__ == "__main__":
    
    # 1. 运行力分配示例
    run_force_distribution_example()
    
    # 2. 运行 Pinocchio 动力学计算
    wbc_inputs = run_pinocchio_dynamics_example()
    
    # 3. 运行 WBC QP 示例
    # 我们需要 16.4 的输出来运行 16.6
    # 假设使用 16.4 示例中的抓握
    r_L_o = np.array([0.5, 0, 0])
    r_R_o = np.array([-0.5, 0, 0])
    G_T = build_grasp_matrix_transpose(r_L_o, r_R_o)
    
    # 假设期望的净力为 0
    W_des = np.zeros(6)
    G_T_pseudo_inv = np.linalg.pinv(G_T) 
    F_p = G_T_pseudo_inv @ W_des
    N_int = scipy.linalg.null_space(G_T)
    
    force_dist_inputs = (G_T, N_int, F_p)

    run_wbc_qp_example(wbc_inputs, force_dist_inputs)