\documentclass[12pt, a4paper]{article}

% ========== 基础包 ==========
\usepackage[UTF8]{ctex}          % 中文支持
\usepackage[margin=2.5cm]{geometry} % 页边距
\usepackage{titlesec}            % 章节标题格式
\usepackage{titletoc}            % 目录格式
\usepackage{fancyhdr}            % 页眉页脚
\usepackage{listings}            % 代码块
\usepackage{xcolor}              % 颜色
\usepackage{graphicx}            % 图片
\usepackage{amsmath}             % 数学公式
\usepackage{amssymb}             % 数学符号
\usepackage{booktabs}            % 表格
\usepackage{enumitem}            % 列表格式
\usepackage{tcolorbox}           % 彩色盒子（用于Q&A）
\usepackage{fontawesome5}        % 图标
\usepackage{setspace}            % 行距
\usepackage{hyperref}            % 超链接（放最后避免与其他包冲突）
\lstdefinestyle{bashstyle}{
    backgroundcolor=\color{codebg},
    basicstyle=\ttfamily\footnotesize,
    breaklines=true,
    frame=single,
    rulecolor=\color{codeframe},
    language=bash,
    commentstyle=\color{codegreen},
    keywordstyle=\color{blue},
}
% ========== 页面设置 ==========
\onehalfspacing                  % 1.5倍行距

% ========== 页眉页脚 ==========
\pagestyle{fancy}
\fancyhf{}
\fancyhead[L]{\small 机器人动作重定向文档}
\fancyhead[R]{\small \leftmark}
\fancyfoot[C]{\thepage}
\renewcommand{\headrulewidth}{0.4pt}

% ========== 超链接设置 ==========
\hypersetup{
    colorlinks=true,
    linkcolor=blue!70!black,
    urlcolor=blue!70!black,
    citecolor=green!60!black,
    bookmarks=true,
    bookmarksnumbered=true
}

% ========== 代码块设置 ==========
\definecolor{codebg}{RGB}{245, 245, 245}
\definecolor{codeframe}{RGB}{200, 200, 200}
\definecolor{codegreen}{rgb}{0,0.6,0}
\definecolor{codegray}{rgb}{0.5,0.5,0.5}
\definecolor{codepurple}{rgb}{0.58,0,0.82}

\lstdefinestyle{pythonstyle}{
    backgroundcolor=\color{codebg},
    commentstyle=\color{codegreen},
    keywordstyle=\color{blue}\bfseries,
    numberstyle=\tiny\color{codegray},
    stringstyle=\color{codepurple},
    basicstyle=\ttfamily\footnotesize,
    breakatwhitespace=false,
    breaklines=true,
    captionpos=b,
    keepspaces=true,
    numbers=left,
    numbersep=5pt,
    showspaces=false,
    showstringspaces=false,
    showtabs=false,
    tabsize=4,
    frame=single,
    rulecolor=\color{codeframe},
    language=Python
}

\lstset{style=pythonstyle}

% ========== Q&A 盒子样式 ==========
\tcbuselibrary{skins, breakable}

\newtcolorbox{qabox}[2][]{
    enhanced,
    breakable,
    colback=blue!5!white,
    colframe=blue!60!black,
    fonttitle=\bfseries,
    title={Q: #2},
    #1
}

\newtcolorbox{answerbox}[1][]{
    enhanced,
    breakable,
    colback=green!5!white,
    colframe=green!60!black,
    leftrule=4pt,
    #1
}

% ========== 文件说明盒子 ==========
\newtcolorbox{filebox}[2][]{
    enhanced,
    breakable,
    colback=gray!10!white,
    colframe=gray!60!black,
    fonttitle=\bfseries\ttfamily,
    title={\faFile\ #2},
    #1
}

% ========== 章节标题格式 ==========
\titleformat{\section}
    {\Large\bfseries\color{blue!70!black}}
    {\thesection}{1em}{}
    [\titlerule]

\titleformat{\subsection}
    {\large\bfseries\color{blue!50!black}}
    {\thesubsection}{1em}{}

\titleformat{\subsubsection}
    {\normalsize\bfseries}
    {\thesubsubsection}{1em}{}

% ========== 文档开始 ==========
\begin{document}

% ========== 封面 ==========
\begin{titlepage}
    \centering
    \vspace*{3cm}
    
    {\Huge\bfseries 机器人动作重定向\\[0.5em]
    \Large 技术文档}
    
    \vspace{2cm}
    \rule{\linewidth}{0.5mm}
    \vspace{1cm}
    
    {\large
    \begin{tabular}{ll}
        \textbf{项目名称：} & 人形机器人动作重定向系统 \\[0.5em]
        \textbf{作者：}     & 沈玉祥 \\[0.5em]
        \textbf{日期：}     & \today \\[0.5em]
        \textbf{版本：}     & v1.0 \\
    \end{tabular}
    }
    
    \vspace{1cm}
    \rule{\linewidth}{0.5mm}
    
    \vfill
    {\small 本文档记录了基于PINK差分IK的人形机器人动作重定向方法及实现细节}
\end{titlepage}

% ========== 目录 ==========
\tableofcontents
\newpage

% ============================================================
% 第一章：程序文件说明
% ============================================================
\section{程序文件说明}

\subsection{概述}
主要包括：人体动作到机器人动作的重定向方法、动作之间的平滑插值、视频提取到机器人动作的方法等内容。
% 在这里写一段整体项目结构的介绍

\subsection{核心文件}

% ---- 文件1 ----
\begin{filebox}{\texttt{ik\_redirection\_npy.py}}
\textbf{功能：} 将人体动作信息转化为机器人各关节角度(rad)信息。\\[0.5em]
\textbf{主要类/函数：}
\begin{itemize}
    \item \texttt{H1Config} —— 机器人配置信息
    \item \texttt{H1PinkSolver} —— 核心求解器
    \item \texttt{process\_motion()} —— 主处理函数
    \item \texttt{apply\_bone\_retargeting\_single()} —— 单帧骨骼重定向
    \item \texttt{load\_data\_all\_npy()} —— 输入格式转换：$[N, 22, 3, T]$ -> 第 i 个 $[T, 22, 3]$
\end{itemize}

\vspace{0.5em}
\textbf{输入：} $[N, 22, 3]$ 的numpy数组，N为帧数，22为SMPL主要关节数，3为XYZ坐标

\textbf{输出：} 字典，主要为dof（关节角度序列，弧度，shape为 $[N, 28]$）

\textbf{运行效率：}0.08s/196帧 $\approx$ 0.4ms/帧（单线程，CPU）
\end{filebox}

\vspace{1em}

% ---- 文件2 ----
\begin{filebox}{\texttt{easy\_MotionInterpolator.py}}
\textbf{功能：} 实现机器人动作（关节角度dof）的插值、帧率重定向及平滑处理；可处理过渡的自碰撞问题。

\vspace{0.5em}
\textbf{主要配置项：}
\begin{itemize}
    \item \texttt{MotionInterpolator} —— 核心插值类
    \item \texttt{interpolate()} —— 对dof序列进行三次样条插值
    \item \texttt{smooth()} —— 加权滑动滤波消除插值引入的抖动
    \item \texttt{process()} —— 依次执行插值与平滑，返回处理后的dof序列
\end{itemize}
\vspace{0.5em}
\textbf{输入：} 低帧率的关节角度序列，shape为 $[N, 28]$

\textbf{输出：} 高帧率平滑后的关节角度序列，shape为 $[N', 28]$

\textbf{运行效率：}单次拼接（含碰撞检测）$\approx$
0.8ms（单线程，CPU）
\end{filebox}

\vspace{1em}

\begin{filebox}{\texttt{vis.py}}
\textbf{功能：} 逐帧播放并可视化。

\vspace{0.5em}
\textbf{主要配置项：}
\begin{itemize}
    \item \texttt{load\_motion\_data()} —— 加载动作数据
    \item \texttt{build\_obstacle\_geoms()} —— 根据障碍物数据构建红色线框盒几何体
    \item \texttt{main()} —— 逐帧绘制机器人关节角度与根节点状态
\end{itemize}
\vspace{0.5em}
\textbf{输入：} 包含dof序列、根节点位姿、可选的h1\_joint\_pos和smpl\_joints\_target

\textbf{输出：} Isaac Gym实时可视化窗口

\textbf{依赖：}Isaac Gym、PyTorch（支持GPU加速渲染）
\end{filebox}

% ---- 添加更多文件 ----
% \begin{filebox}{\texttt{文件名.py}}
% ...
% \end{filebox}

\subsection{服务器端文件}
\textbf{位置：} 服务器下的GVHMR目录下；conda activate gvhmr

\begin{filebox}{\texttt{GVHMR/run.py}}
\textbf{功能：} 把人体动作视频(mp4)转换成机器人各关节电机角度(rad)。

\vspace{0.5em}
\textbf{主要配置项：}
\begin{itemize}
    \item \texttt{GVHMRSystem} —— 核心系统类
    \item \texttt{\_\_init\_\_()} ——  冷启动初始化，加载GVHMR主模型、Tracker、VitPose、特征提取器、SMPL模型及H1PinkSolver
    \item \texttt{process\_video()} —— 热运行入口，处理单个视频，依次执行预处理、GVHMR推理、H1 IK优化，返回关节角度结果
    \item \texttt{\_run\_preprocess\_efficient()} —— SLAM后台线程并行，Tracker、VitPose、特征提取串行GPU推理
    \item \texttt{\_run\_h1\_optimization()} —— 从GVHMR输出的SMPL参数提取关节坐标，调用H1PinkSolver完成重定向
\end{itemize}
\vspace{0.5em}
\textbf{输入：} mp4视频文件

\textbf{输出：} 包含dof关节角度序列（rad，shape为$[N,28]$）的字典

\textbf{使用方法：}
\begin{lstlisting}[style=bashstyle]
conda activate gvhmr
cd GVHMR/
python run.py --input (视频路径)
# 结果存储到 outputs 目录的 result.pkl 文件
\end{lstlisting}

\textbf{运行效率：}4.5s/8s（4090GPU）
\end{filebox}

\vspace{1em}

\begin{filebox}{\texttt{GVHMR/new-server.py}}
\textbf{功能：} 支持大模型工具调用的服务端。

\vspace{0.5em}
\textbf{主要配置项：}
\begin{itemize}
    \item \texttt{\_run\_gvhmr()} —— 核心推理函数
    \item \texttt{\_frames\_to\_tmp\_video()} —— 将帧列表写入临时视频文件
    \item \texttt{\_process\_frames\_async()} —— 异步推理包装
\end{itemize}
\vspace{0.5em}

\textbf{主要接口：}
\begin{itemize}
    \item \lstinline{GET /tools} —— 查询当前可用工具，大模型调用前先请求：
    \begin{lstlisting}[style=bashstyle]
curl http://localhost:8003/tools\end{lstlisting}
    \item \lstinline{POST /tool/call} —— 大模型调用工具的入口，传工具名和参数：
    \begin{lstlisting}[style=bashstyle]
curl -X POST http://localhost:8003/tool/call \
     -d '{"name": "process_video_to_robot_motion", 
          "parameters": {"video_path": "/data/demo.mp4"}}'\end{lstlisting}
    \item \lstinline{POST /generate} —— 直接输入视频文件路径处理：
    \begin{lstlisting}[style=bashstyle]
curl -X POST http://localhost:8003/generate \
     -d '{"video_path": "/data/demo.mp4"}'\end{lstlisting}
    \item \lstinline{WS /ws/stream} —— 接入实时相机推流，配合 camera\_client.py 使用：
    \begin{lstlisting}[style=bashstyle]
python camera_client.py --camera 0\end{lstlisting}
\end{itemize}
\vspace{0.5em}
\textbf{输入：} 视频文件绝对路径，或通过 WebSocket 推送的 JPEG 编码帧字节流

\textbf{输出：} 字典，主要为 dof（关节角度序列，弧度，shape 为 $[N,28]$）、fps、duration\_sec
\end{filebox}

\vspace{1em}

\begin{filebox}{\texttt{GVHMR/camera\_client.py}}
\textbf{功能：} 获取本地摄像头，将视频帧实时推送到 new-server.py 进行推理，并接收返回的关节角度。\\[0.5em]
\textbf{主要函数：}
\begin{itemize}
    \item \texttt{run\_camera\_stream()} —— 打开摄像头、推帧、接收结果
    \item \texttt{\_handle\_result()} —— 处理每次推理返回的结果
\end{itemize}

\vspace{0.5em}
\textbf{使用方法：}
\begin{lstlisting}[style=bashstyle]
# 默认摄像头，默认参数
python camera_client.py

# 指定摄像头编号
python camera_client.py --camera 0 --fps 30 --chunk_sec 4

# 服务端不在同一台机器时指定地址
python camera_client.py --server 192.168.1.100:8003

# 固定机位相机
python camera_client.py --static_cam
\end{lstlisting}

\textbf{输入：} 本地摄像头实时画面（通过 OpenCV 读取）\\[0.5em]
\textbf{输出：} 每隔 \texttt{chunk\_sec} 秒打印一次推理结果，格式如下：
\begin{lstlisting}[style=bashstyle]
[推理结果] chunk_id=0 | 帧数=120 | 时长=4.0s | fps=90 | DOF shape=[360, 28]
\end{lstlisting}

\textbf{备注：} \texttt{\_handle\_result()} 函数内的注释处（第~117~行）可对接机器人控制器，将 DOF 数据发给 ROS 节点。
\end{filebox}

\vspace{1em}

\begin{filebox}{\texttt{GVHMR/test\_stream.py}}
\textbf{功能：} 用本地 mp4 视频文件模拟摄像头推流，测试 new-server.py 的视频流接口是否正常工作。\\[0.5em]
\textbf{主要函数：}
\begin{itemize}
    \item \texttt{test\_stream()} —— 读取视频文件逐帧推送到服务端，并接收推理结果
    \item \texttt{\_print\_result()} —— 打印每个 chunk 的推理结果
\end{itemize}

\vspace{0.5em}
\textbf{使用方法：}
\begin{lstlisting}[style=bashstyle]
# 基本用法（必须指定视频文件）
python test_stream.py --video /data/demo.mp4

# 指定参数
python test_stream.py --video /data/demo.mp4 --fps 30 --chunk_sec 4

# 服务端不在同一台机器时指定地址
python test_stream.py --video /data/demo.mp4 --server 192.168.1.100:8003
\end{lstlisting}

\textbf{输入：} 本地视频文件路径\\[0.5em]
\textbf{输出：} 每积累 \texttt{chunk\_sec} 秒的帧触发一次推理，打印结果：
\begin{lstlisting}[style=bashstyle]
[chunk 0] dof_shape=[360, 28], fps=90, duration=4.0s
[done] 共处理 2 个 chunk
\end{lstlisting}
\end{filebox}

\subsection{其他说明}

\begin{itemize}
    \item \textbf{根节点固定：} 本模块的目标是从人体动作生成机器人电机角度序列，机器人waist点在世界坐标系中保持固定，相当于将机器人腰部悬挂于空间中。根节点的全局位移不参与重定向计算。

    \item \textbf{输出崩溃排查：} 若输出的电机角度出现突变或崩溃，大概率是输入动作幅度过大，超出机器人关节限位范围，导致IK无法在单帧内追上目标。解决方法是适当降低大模型生成的动作幅度，或延长动作时长使运动更平缓。

    \item \textbf{体型通用性：} 重定向模块对输入人体的臂长、身高、体型无要求，骨骼重定向阶段会自动将SMPL骨骼方向映射到机器人实际骨骼长度，不受输入人体比例影响。
\end{itemize}
% 在这里描述其他辅助脚本


\newpage

% ============================================================
% 第二章：重定向方法与原理
% ============================================================
\section{重定向方法与原理}
\subsection{常见的三种重定向方法}
常见的机器人动作重定向方法主要分为三类：梯度下降方法、神经网络模型+IK微调、纯IK逆解方法三种。

\begin{table}[h]
\centering
\caption{三种重定向方法对比}
\begin{tabular}{p{2.5cm}p{4cm}p{4cm}p{3cm}}
\toprule
\textbf{方法} & \textbf{优点} & \textbf{缺点} & \textbf{适用场合} \\
\midrule
梯度下降 &
动作质量高；批量处理性价比好 &
实时性差，处理数秒视频需2s以上；需预先标定缩放比例 &
离线批量处理 \\
\midrule
神经网络 + IK微调 &
推理速度快；兼顾数据驱动与几何约束 &
依赖大量高质量标注数据，准备成本高；对体型变化鲁棒性弱 &
数据充足的生产环境 \\
\midrule
纯IK逆解（本文） &
无需训练数据；实时求解；体型适应性好 &
需合理设计任务约束与权重 &
实时重定向 \\
\bottomrule
\end{tabular}
\end{table}

\subsection{整体流程}

本系统的重定向流程如下：

\begin{center}
\begin{tcolorbox}[width=0.85\textwidth, colback=blue!5, colframe=blue!40, 
                  boxrule=0.5pt, arc=3pt]
\centering
\textbf{输入} SMPL关节坐标序列 $[N, J, 3]$ \\[4pt]
$\downarrow$ \\[2pt]
坐标系变换（quat旋转，对齐机器人坐标系）\\[4pt]
$\downarrow$ \\[2pt]
根节点归一化 $\rightarrow$ 比例缩放 $\rightarrow$ 平移至waist高度 \\[4pt]
$\downarrow$ \\[2pt]
Yaw朝向对齐（固定根节点朝向）\\[4pt]
$\downarrow$ \\[2pt]
Savitzky-Golay 平滑滤波 \\[4pt]
$\downarrow$ \\[2pt]
设定IK追踪任务与权重 \\[4pt]
$\downarrow$ \\[2pt]
逐帧骨骼重定向（动态Anchor + 骨骼长度替换）\\[4pt]
$\downarrow$ \\[2pt]
QP差分IK求解 \\[4pt]
$\downarrow$ \\[2pt]
加权滑动窗口平滑 $\rightarrow$ 脚踝pitch几何覆写 \\[4pt]
$\downarrow$ \\[2pt]
三次样条插值升采样至90fps \\[4pt]
$\downarrow$ \\[2pt]
\textbf{输出} 关节角度序列（rad，$[N, 28]$）
\end{tcolorbox}
\end{center}

% 描述整体 pipeline，可以用 itemize 列出步骤

\subsection{坐标系处理}

\subsubsection{SMPL坐标系与机器人坐标系的对齐}

SMPL以骨盆（pelvis）为根节点，机器人以waist为躯干中心，两者物理意义最接近，因此将pelvis对齐到机器人waist位置。具体分两步：以pelvis为原点中心化后乘以缩放系数，再平移至机器人waist高度：

\begin{lstlisting}[style=bashstyle]
# 中心化 + 缩放
smpl_joints = (smpl_raw_seq - smpl_raw_seq[:, 0:1, :]) * self.SMPL_SCALE
# 平移至机器人waist高度
waist_pos = self.configuration.get_transform_frame_to_world("waist").translation
smpl_joints += waist_pos.reshape(1, 1, 3)
\end{lstlisting}

% 描述 quat = [0.5, 0.5, 0.5, 0.5] 的作用

\subsubsection{朝向对齐预处理}

大模型输出的动作可能包含根节点移动（如走圈），直接重定向会导致机器人跟随转向。因此对每帧计算髋部左右向量的水平偏转角，绕Z轴旋转补偿，使全程朝向与第0帧保持一致：

\begin{lstlisting}[style=bashstyle]
# 计算每帧髋部向量的yaw角
hip_vecs = smpl_joints[:, idx_r] - smpl_joints[:, idx_l]
hip_vecs[:, 2] = 0  # 投影到水平面
yaw_ref = np.arctan2(hip_vecs[0, 1], hip_vecs[0, 0])
yaw_all = np.arctan2(hip_vecs[:, 1], hip_vecs[:, 0])

# 绕Z轴旋转补偿，固定朝向
rot_mats = sRot.from_euler('z', yaw_ref - yaw_all).as_matrix()
centered = smpl_joints - smpl_joints[:, 0:1, :]
smpl_joints = np.einsum('nij,nkj->nki', rot_mats, centered) + smpl_joints[:, 0:1, :]
\end{lstlisting}

% 描述固定根节点朝向的处理逻辑

\subsection{骨骼重定向}
\begin{figure}[h]
\centering
\begin{minipage}{0.45\textwidth}
    \centering
    \includegraphics[width=\textwidth]{before.png}
    \caption{重定向前}
\end{minipage}
\hfill
\begin{minipage}{0.45\textwidth}
    \centering
    \includegraphics[width=\textwidth]{after.png}
    \caption{重定向后}
\end{minipage}
\caption{重定向前后对比：绿色为H1机器人关节点，蓝色为SMPL目标关节点}
\label{fig:retarget_compare}
\end{figure}
\subsubsection{动态Anchor设计}

SMPL人体与机器人结构不同，肩部和髋部的关节位置无法直接对应。若直接以SMPL的肩膀点作为anchor，会有两个问题：一是两者骨骼比例不同导致位置偏差；二是人体上肢挥动时SMPL肩膀点存在上下位移，而机器人肩部三个电机的结构无法复现这种位移。

因此每帧从机器人当前正运动学结果中读取肩部、髋部、颈部的真实世界坐标作为anchor，再以SMPL骨骼的方向信息驱动末端位置：

\begin{lstlisting}[style=bashstyle]
# 每帧从机器人正运动学读取anchor点
anchors = {
    'l_shoulder': self.configuration.get_transform_frame_to_world(
                      "left_upper_arm").translation.copy(),
    'r_shoulder': self.configuration.get_transform_frame_to_world(
                      "right_upper_arm").translation.copy(),
    'l_thigh':    self.configuration.get_transform_frame_to_world(
                      "left_thigh").translation.copy(),
    'r_thigh':    self.configuration.get_transform_frame_to_world(
                      "right_thigh").translation.copy(),
    'neck':       self.configuration.get_transform_frame_to_world(
                      "neck_linkage").translation.copy(),
}
\end{lstlisting}

同时，利用SMPL的spine1、left\_collar、right\_collar三点构造正交基，得到躯干朝向，用于约束机器人胸部旋转，间接驱动上肢姿态跟随躯干运动：

\begin{lstlisting}[style=bashstyle]
# 三点构造胸部正交基
up      = _normalize((p_l_collar + p_r_collar) / 2.0 - p_spine3)
right   = _normalize(p_l_collar - p_r_collar)
forward = _normalize(np.cross(right, up))
right   = _normalize(np.cross(up, forward))
chest_rot = np.column_stack([forward, right, up])
\end{lstlisting}

% 描述为什么要用动态anchor，以及如何实现

\subsubsection{链式骨骼长度替换}

SMPL与机器人的骨骼长度不同，直接使用SMPL关节坐标会导致末端位置偏差。解决方案是保留SMPL各骨骼段的方向向量，将长度替换为机器人实际物理尺寸，从anchor点出发链式计算各关节位置，使末端（手、脚）能够正确对齐：

\begin{lstlisting}[style=bashstyle]
# 机器人实际骨骼长度（米）
ROBOT_DIMS = {
    'thigh':     0.0832,   # 髋 -> 膝
    'calf':      0.1105,   # 膝 -> 踝
    'upper_arm': 0.07579,  # 肩 -> 肘
    'forearm':   0.04739,  # 肘 -> 腕
    'neck2head': 0.022487329, # 颈部 -> 头
}

# 以anchor为父节点，沿SMPL方向链式扩展（以左腿为例）
nj[:, b['left_hip']]   = anchors['l_thigh']
nj[:, b['left_knee']]  = anchors['l_thigh'] \
    + _normalize(j[:, b['left_knee']]  - j[:, b['left_hip']])  * d['thigh']
nj[:, b['left_ankle']] = nj[:, b['left_knee']] \
    + _normalize(j[:, b['left_ankle']] - j[:, b['left_knee']]) * d['calf']
\end{lstlisting}

左右臂同理，从肩部anchor出发，依次计算肘、腕位置。经过链式替换后，末端关节坐标既保留了SMPL动作的姿态方向，又符合机器人的实际骨骼比例。

% 描述 apply_bone_retargeting_single 的逻辑

\subsection{差分IK求解}

\subsubsection{PINK框架介绍}

PINK（Python Inverse kiNematics）是基于Pinocchio的差分IK框架。Pinocchio负责机器人运动学/动力学计算，PINK在其基础上将IK问题构造为二次规划（QP）问题，通过定义多个带权重的\textbf{任务}（Task）和\textbf{约束}（Limit）来描述求解目标。

每个任务对应一个末端执行器的位置或旋转目标，权重决定各任务的优先级。求解器在满足关节限位约束的前提下，每帧求解一个关节速度增量，再积分更新机器人配置，从而逐帧追踪目标轨迹。本系统使用quadprog作为底层QP求解器，阻尼系数设为$10^{-3}$防止奇异。

% 简要介绍 PINK 和 Pinocchio

\subsubsection{任务设计与权重}

系统共定义以下IK任务，各任务的位置权重与旋转权重如表所示：

\begin{table}[h]
\centering
\caption{IK任务权重配置}
\begin{tabular}{llcc}
\toprule
\textbf{机器人Link} & \textbf{对应SMPL关节} & \textbf{位置权重} & \textbf{旋转权重} \\
\midrule
chest           & —            & 0.0  & 50.0 \\
left\_hand / right\_hand   & left\_wrist / right\_wrist  & 10.0 & 0.0  \\
left\_foot / right\_foot   & left\_ankle / right\_ankle  & 10.0 & 1.0  \\
left\_force\_arm / right\_force\_arm & left\_elbow / right\_elbow & 5.0 & 0.0 \\
left\_calf / right\_calf   & left\_knee / right\_knee    & 5.0  & 0.0  \\
head            & head         & 1.0  & 5.0  \\
\bottomrule
\end{tabular}
\end{table}

chest任务不追位置，只追旋转，用于约束躯干朝向防止后仰；手和脚作为末端执行器位置权重最高；肘和膝作为中间关节权重较低，主要用于引导肢体方向；头部以旋转约束为主。此外还加入了一个低权重（0.1）的PostureTask作为软约束，防止未被追踪的关节乱飘：

\begin{lstlisting}[style=bashstyle]
self.posture_task = pink.tasks.PostureTask(self.model)
self.posture_task.set_target(self.q_ref)
self.posture_task.cost = 0.1
\end{lstlisting}

% 描述各个 FrameTask 的权重设计思路

\subsubsection{躯干朝向约束}

IK求解时若不对躯干朝向加以约束，机器人上半身容易出现后仰或侧倾。为此单独设置一个chest的FrameTask，只追旋转不追位置（位置权重为0，旋转权重为50），每帧从SMPL的spine1、left\_collar、right\_collar三点构造正交基作为目标旋转矩阵（见2.4.1节），强制机器人胸部朝向与SMPL躯干保持一致。

由于位置不强制追踪，chest的世界坐标始终取机器人当前真实位置，避免与其他任务产生冲突：

\begin{lstlisting}[style=bashstyle]
chest_pos = self.configuration.get_transform_frame_to_world("chest").translation.copy()
self.chest_task.set_target(pin.SE3(chest_rot, chest_pos))
\end{lstlisting}

% 描述用 spine3/left_collar/right_collar 三点确定胸部朝向的方法

\subsubsection{头部朝向处理}

头部朝向通过两种方式处理。若上游提供了\lstinline{head_rot_mats}（局部旋转矩阵），则将其与当前帧的躯干旋转矩阵相乘得到头部世界朝向；若未提供则只追位置，旋转使用单位矩阵：

\begin{lstlisting}[style=bashstyle]
if smpl_name == "head" and head_rot_mats is not None:
    # 世界朝向 = 躯干朝向 x 头部局部旋转
    task.set_target(pin.SE3(chest_rot @ head_rot_mats[i], limited_targets[smpl_name]))
else:
    task.set_target(pin.SE3(np.eye(3), limited_targets[smpl_name]))
\end{lstlisting}

由于\lstinline{head_rot_mats}是相对于父关节的局部旋转，对于包含大幅根节点移动的动作（如走圈），预处理阶段会自动检测并清除旋转向量的Yaw分量，避免头部跟随身体转向产生突变。

% 描述头部朝向的处理方法

\subsection{数据处理与平滑}

\subsubsection{插值扩帧}

IK求解在20fps上运行，完成后对关节角度序列做三次样条插值升采样至90fps。在关节空间插值比在笛卡尔空间插值更物理合理，且计算开销小：

\begin{lstlisting}[style=bashstyle]
interp = interp1d(t_orig, sim_dof, axis=0, kind='cubic')
sim_dof = interp(t_new)
\end{lstlisting}

% 描述从20fps插值到90fps的方法

\subsubsection{加权滑动滤波}

对IK输出的关节角度序列做线性加权滑动均值滤波（窗口7帧），越近的帧权重越高，有效抑制IK求解引入的高频抖动。该滤波方法在机器人动作重定向领域被广泛采用：

\begin{lstlisting}[style=bashstyle]
w = np.arange(1, window_size + 1, dtype=float)
w /= w.sum()  # 线性权重归一化，越新的帧权重越大
\end{lstlisting}

% 描述 WeightedMovingFilter

\subsubsection{关节限位约束}

在IK求解时加入ConfigurationLimit约束，将28个关节的角度限制在物理允许范围内，防止求解结果超出机器人实际关节行程：

\begin{lstlisting}[style=bashstyle]
# 将限位写入Pinocchio模型
self.model.lowerPositionLimit[q_idx] = H1Config.JOINT_LIMITS[idx, 0]
self.model.upperPositionLimit[q_idx] = H1Config.JOINT_LIMITS[idx, 1]
self.config_limit = ConfigurationLimit(self.model)

# IK求解时传入约束
velocity = pink.solve_ik(
    self.configuration, self.tasks, dt,
    solver="quadprog", damping=1e-3,
    limits=[self.config_limit],
)
\end{lstlisting}

% 描述 ConfigurationLimit 的使用

\subsubsection{脚踝角度计算}

机器人没有脚掌关节，脚踝pitch角度无法通过IK直接求解。因此从SMPL几何信息中提取每帧小腿方向向量与脚掌方向向量的夹角，以第0帧（站立姿态）为基准计算相对变化量，直接覆写到脚踝pitch电机：

\begin{lstlisting}[style=bashstyle]
# 计算小腿与脚掌方向向量的夹角
shin  = _normalize(ankle - knee)
toes  = _normalize(foot  - ankle)
angle = np.arccos(np.clip((shin * toes).sum(axis=-1), -1.0, 1.0))

# 以第0帧为中性角基准，限制在关节限位内
pitch = angle - angle[0]
pitch = np.clip(pitch, lo, hi)
\end{lstlisting}

% 描述从小腿/脚掌方向向量计算脚踝pitch角的方法

\subsection{其他}

\subsubsection{遥操作与实时重定向的区别}

目前业内已有端到端的实时遥操作方案（如GR-1、AnyTeleop等），直接从摄像头到机器人控制信号，延迟极低。本系统与其定位不同，对比如下：

\begin{table}[h]
\centering
\caption{实时遥操作方案与本系统对比}
\begin{tabular}{p{3cm}p{5.5cm}p{5.5cm}}
\toprule
 & \textbf{实时遥操作（如GMR）} & \textbf{本系统（GVHMR + IK）} \\
\midrule
\textbf{输入来源} & 实时摄像头 & 视频文件或大模型npy输出 \\
\textbf{实时性} & 极低延迟，在线处理 & 离线为主，单帧$\approx$0.4ms \\
\textbf{精度} & 人体捕捉精度较低，但足够还原动作 & GVHMR精度较高 \\
\textbf{机器人泛化性} & 换机器人需重新训练模型 & 只需修改URDF和骨骼参数，无需重训 \\
\textbf{适用场景} & 实时控制、遥操作 & 动作生成、离线批量处理 \\
\textbf{主要优势} & 延迟低、部署简单 & 通用性强，兼容多种输入源 \\
\bottomrule
\end{tabular}
\end{table}

本系统优先接入大模型生成的npy动作数据，以通用IK重定向为核心，机器人结构发生变化时只需更新URDF和骨骼长度参数即可适配，无需重新训练任何模型，维护成本低。

\subsubsection{局部四肢避障方法}


在动作模仿过程中，若摄像头检测到障碍物与机器人四肢存在碰撞风险，需要对目标关节点位进行实时修正。

\textbf{基本思路}：不修改IK框架，而是在骨骼重定向阶段对SMPL关节点位进行几何修正，将避障后的新点位作为IK追踪目标传入。

\textbf{几何避障原理}：以手臂为例，肩膀、肘部、手腕三点构成一个两边长度固定（大臂长、小臂长）的运动链。当大臂或小臂检测到与障碍物碰撞时，在保持骨骼长度不变的约束下，迭代计算绕障后的新关节位置，使整个手臂绕过障碍物的同时保持人体物理结构不变形。腿部同理，由髋、膝、踝三点构成固定长度运动链做同样处理。

\textbf{局限性}：该方法基于纯几何计算，时效性好，适用于简单的局部避障场景。对于需要腰部扭转、重心转移等全身协调配合的复杂避障情况，单纯修正四肢点位无法保证整体姿态合理性，需要引入更完整的全身运动规划方案。

\begin{figure}[h]
\centering
\includegraphics[width=\textwidth]{arm_avoidance.png}
\caption{局部四肢避障几何方法示意图}
\label{fig:arm_avoidance}
\end{figure}
% 在这里写你对整个重定向方法的理解，遇到的问题和解决思路

\newpage

% ============================================================
% 第三章：问答记录
% ============================================================
\section{常见踩坑}

\begin{qabox}{quat旋转的作用是什么？}
\end{qabox}

\begin{answerbox}
大模型输出的SMPL关节坐标以Y轴朝上为标准坐标系，而本系统机器人重定向以Z轴朝上为基准。若不做坐标系变换，输入的SMPL人体在机器人坐标系下呈躺倒状态，无法正常重定向。

通过四元数 $q=[0.5, 0.5, 0.5, 0.5]$ 对应的旋转矩阵，将Y轴朝上变换为Z轴朝上：

\begin{lstlisting}[style=bashstyle]
rot = sRot.from_quat([0.5, 0.5, 0.5, 0.5]).as_matrix()
smpl_raw_seq = target_motion @ rot.T  # [T, J, 3]
\end{lstlisting}

对于GVHMR视频提取的SMPL数据，输出已是Z轴朝上，无需做Y轴到Z轴的变换，只需绕Z轴旋转$-90°$调整朝向，对应quat参数为$[0, 0, -0.707, 0.707]$ （参考服务器端GVHMR目录下的逆解文件）。
\end{answerbox}
\vspace{1em}

% ---- 继续添加 Q&A ----

\subsection{骨骼重定向相关}

\begin{qabox}{若不将躯干与两臂任务解耦会有什么后果？}
\end{qabox}

\begin{answerbox}
若直接以SMPL肩膀点作为两臂IK追踪的anchor，由于机器人与SMPL骨骼比例不同，肩膀点无法完全对齐，导致以肩膀为起点的手肘、手腕等末端追踪点整体偏移，IK始终存在残差。

更严重的问题是，PINK本质上求解的是QP问题，当多个任务存在冲突时，求解器会在各任务间做权重妥协。若躯干与手臂任务耦合，某一侧手臂恰好能追上目标而另一侧追不上时，QP求解器为了整体最优会将误差集中到某个关节，导致该关节的轴向电机出现突变甚至疯转。

通过动态Anchor设计，每帧以机器人自身正运动学的肩膀位置为起点，将躯干运动与手臂末端追踪解耦，从根本上避免了上述问题。
\end{answerbox}
% Q&A

\subsection{IK求解相关}

\begin{qabox}{为什么不先插值到90fps再进行IK求解？}
\end{qabox}

\begin{answerbox}
IK求解的耗时与帧数成正比，若先插值到90fps再求解，计算量是当前方案的4.5倍。而先在20fps上完成IK求解，再对关节角度序列做三次样条插值升采样至90fps，精度损失极小，因为相邻帧间的关节角度变化平滑，插值能够很好地还原中间帧。

因此选择先IK后插值的方案，在几乎不影响动作质量的前提下将IK阶段耗时降低至原来的$\frac{1}{4.5}$，是性价比最高的做法。
\end{answerbox}

\begin{qabox}{为什么不在IK求解过程中加入速度控制？}
\end{qabox}

\begin{answerbox}
主要有三个原因：

第一，职责解耦。重定向模块只负责将人体动作映射为机器人关节角度，速度控制属于运动控制层的职责，两者混合会导致模块边界模糊，出现问题时难以定位。

第二，仿真与现实存在差距。PINK求解的是仿真环境下的机器人状态，加入速度控制后轨迹会产生延迟跟踪效果，而这个延迟在真实机器人上的表现与仿真并不一致，反而引入额外误差。

第三，需求已满足。本系统的目标是离线生成某段动作对应的机器人关节角度序列，并不需要在线实时跟踪，因此静态的角度序列输出已经完全满足需求，无需引入速度控制增加复杂度。
\end{answerbox}


% Q&A

% ============================================================
% 附录
% ============================================================
\newpage
\section{后续规划}
\subsection{目标}

\begin{itemize}
    \item \textbf{训练机器人行走：} 利用重定向模块生成的高质量行走动作序列作为参考轨迹，通过强化学习训练机器人在仿真环境中实现稳定行走。

    \item \textbf{去掉固定根节点限制：} 当前重定向模块输出的电机角度序列以固定waist为前提，无法直接在真实环境中执行带位移的动作。目标是结合强化学习，训练机器人在真实动力学约束下完成动作模仿，使其能够在现实环境中真正执行出对应动作。
\end{itemize}

\subsection{计划路线}
当前调研的技术路线如下：

\begin{center}
\begin{tcolorbox}[width=0.85\textwidth, colback=blue!5, colframe=blue!40,
                  boxrule=0.5pt, arc=3pt]
\centering
\textbf{输入} 大模型生成 / 视频提取 \\[4pt]
$\downarrow$ \\[2pt]
转换为SMPL关节坐标序列（.npy） \\[4pt]
$\downarrow$ \\[2pt]
动作重定向模块 $\rightarrow$ 输出电机角度序列（dof） \\[4pt]
$\downarrow$ \\[2pt]
接入强化学习模块 \\[2pt]
（结合动力学信息对角度序列进行调整与优化） \\[4pt]
$\downarrow$ \\[2pt]
\textbf{输出} 可在真实环境执行的机器人动作
\end{tcolorbox}
\end{center}

\subsection{参考方法}

\begin{itemize}
    \item \textbf{humanoid-gym}：基于PPO的H1行走训练，用于验证仿真环境与机器人配置。
    \item \textbf{ExBody2}：基于AMP框架的动作模仿，以重定向模块输出的dof序列作为参考轨迹，对重定向误差容忍度较高。
    \item \textbf{OmniH2O}：全身精确动作模仿备选方案，关节角度直接跟踪，还原度更高但对重定向精度要求更高。
\end{itemize}

\newpage
\appendix

\section{关键参数说明}

\begin{table}[h]
\centering
\caption{重定向关键参数}
\begin{tabular}{lll}
\toprule
\textbf{参数名} & \textbf{值} & \textbf{说明} \\
\midrule
\texttt{scale}      & 0.2466  & SMPL到机器人的缩放比例 \\
\texttt{aim\_fps}   & 90      & 目标帧率 \\
\texttt{substeps}   & 2       & IK每帧子步数 \\
\texttt{max\_step}  & 0.01    & 目标点每步最大移动距离(m) \\
\texttt{damping}    & 1e-3    & IK阻尼系数 \\
\bottomrule
\end{tabular}
\end{table}

\end{document}