robot_lab中amp_utils——retarget_kp_motions.py解析

详细解析

main函数

1.仿真环境初始化

（1） 连接 PyBullet

p = pybullet
if config.VISUALIZE_RETARGETING:p.connect(p.GUI, options='--width=1920 --height=1080 --mp4="test.mp4" --mp4fps=60')
else:p.connect(p.DIRECT, options='--width=1920 --height=1920 --mp4="test.mp4" --mp4fps=60')

选择连接方式：
根据配置参数 config.VISUALIZE_RETARGETING，决定使用 GUI 模式（可视化仿真）还是 DIRECT 模式（无头运行）。参数说明：--width=1920 --height=1080 设置仿真窗口的分辨率。--mp4="test.mp4" --mp4fps=60 指定录制视频的文件名和帧率。

（2）配置调试渲染与资源路径

p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING, 1)
pybullet.setAdditionalSearchPath(pd.getDataPath())

p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING, 1)

作用：
启用单步渲染模式。解释：
通过调用 configureDebugVisualizer 函数，并传入参数 p.COV_ENABLE_SINGLE_STEP_RENDERING 与值 1，告诉 PyBullet 在调试模式下每一步都单独渲染，而不是自动连续渲染。效果：
这在调试时非常有用，因为可以确保每一次状态更新后画面都能稳定地显示，方便检查仿真过程中各个步骤的效果。

pybullet.setAdditionalSearchPath(pd.getDataPath())

作用：
添加额外的资源搜索路径。解释：
pd.getDataPath() 是 PyBullet 提供的一个函数，用于返回 PyBullet 自带数据文件（如 URDF、模型等）的默认目录。
调用 pybullet.setAdditionalSearchPath 后，PyBullet 在加载文件（例如 URDF 文件）时会自动搜索该目录。效果：
使得在调用诸如 pybullet.loadURDF 等加载函数时，不必提供完整路径，PyBullet 能够自动在这个附加目录中查找资源文件。

（3）输出目录准备

output_dir = config.OUTPUT_DIR
if not os.path.exists(output_dir):os.makedirs(output_dir)

创建输出目录：
确保配置文件中指定的输出目录存在，用于保存最终生成的运动数据文件。

2.循环处理每个动作捕捉（mocap）动作

（1） 遍历配置中的每个动作

for mocap_motion in config.MOCAP_MOTIONS:

mocap_motion：
每个动作捕捉数据包含动作名称和多个参数（例如数据文件路径、帧起始和结束索引以及其它参数）。

（2）仿真重置与设置

pybullet.resetSimulation()
pybullet.setGravity(0, 0, 0)ground = pybullet.loadURDF(GROUND_URDF_FILENAME)
robot = pybullet.loadURDF(config.URDF_FILENAME,config.INIT_POS,config.INIT_ROT,flags=p.URDF_MAINTAIN_LINK_ORDER,
)

重置仿真：
重置当前仿真环境，清除之前的物体和状态。设置重力：
将重力设为 (0, 0, 0)（可能是为了专注于运动重定向而不引入重力影响）。加载地面和机器人：
分别加载地面模型和机器人模型。机器人初始位置、朝向由 config.INIT_POS 和 config.INIT_ROT 指定。

（3）设置机器人初始状态

set_pose(robot,np.concatenate([config.INIT_POS, config.INIT_ROT, config.DEFAULT_JOINT_POSE]),
)

目的：
通过拼接初始位置、旋转和默认关节角度，将机器人置于一个合理的默认姿态，为后续运动提供良好初始状态。

（4）读取与预处理运动数据

print(f"Re-targeting {mocap_motion}")
p.removeAllUserDebugItems()joint_pos_data = load_ref_data(mocap_motion[1], mocap_motion[2], mocap_motion[3] + 1
)
if "reverse" in mocap_motion[0]:joint_pos_data = np.flip(joint_pos_data, axis=0)

• 输出动作名称

print(f"Re-targeting {mocap_motion}")

作用：
使用 f-string 输出当前正在处理的动作捕捉数据的信息。解释：
mocap_motion 变量通常包含动作名称或其他描述信息，这里打印出来便于在控制台中了解当前处理进度和具体动作标识。

• 移除所有调试对象

p.removeAllUserDebugItems()

作用：
清除 PyBullet 调试视图中所有之前添加的调试对象。解释：
在仿真过程中，为了可视化调试可能会添加各种调试标记或线条。调用 removeAllUserDebugItems() 能够清空这些调试对象，避免它们干扰后续新的调试信息显示。

• 加载参考数据

joint_pos_data = load_ref_data(
mocap_motion[1], mocap_motion[2], mocap_motion[3] + 1
)

作用：
调用自定义函数 load_ref_data 从文件中加载参考关节位置数据。解释：参数 mocap_motion[1]：通常是参考数据文件的路径。参数 mocap_motion[2]：参考数据的起始帧索引。参数 mocap_motion[3] + 1：参考数据的结束帧索引（+1 的目的是包含指定帧）。这样得到的 joint_pos_data 是一个数组，存储了动作捕捉文件中的关节数据，每一行通常代表一帧数据。

• 数据反转（如果需要）

if “reverse” in mocap_motion[0]:
joint_pos_data = np.flip(joint_pos_data, axis=0)

作用：
检查动作名称中是否包含 "reverse" 字样，如果包含则对数据进行反转。解释：通过 if "reverse" in mocap_motion[0]: 判断动作名称（mocap_motion[0]）中是否含有 "reverse"。若条件满足，调用 np.flip(joint_pos_data, axis=0) 将整个数组沿着帧的维度（行）翻转。这通常用于处理那些需要反向播放或反向运动的动作数据，使得原始帧顺序颠倒。

（5）数据格式调整与坐标转换

joint_pos_data = joint_pos_data.reshape(joint_pos_data.shape[0], -1, POS_SIZE)
for i in range(joint_pos_data.shape[0]):joint_pos_data[i] = process_ref_joint_pos_data(joint_pos_data[i])

重排数据：
将数据 reshape 成 [帧数, 关键点个数, 3] 的形状，每个关键点由 3 个坐标组成。坐标转换：
对每一帧数据调用 process_ref_joint_pos_data，进行旋转、缩放和平移等操作，使数据与仿真环境坐标系一致。

（6）调整足端高度

for ref_toe_joint_id in REF_TOE_JOINT_IDS:joint_pos_data[:, ref_toe_joint_id, -1] -= np.min(joint_pos_data[:, ref_toe_joint_id, -1])print(joint_pos_data[:, ref_toe_joint_id, -1])joint_pos_data[:, ref_toe_joint_id, -1] += config.TOE_HEIGHT_OFFSET

对于每个脚尖关节（最后一维表示 z 轴高度），先将所有帧中该关节的高度下移，
使最低值为零，再加上一个常数 config.TOE_HEIGHT_OFFSET。这样做可以确保在仿
真时足端与地面的接触正确，避免足端穿地或高度过低问题。

3. 运动重定向与数据输出

（1）计算重定向后的运动数据

retarget_frames = retarget_motion(robot, joint_pos_data)
joint_pos_data = joint_pos_data[:-1, :]

retarget_motion：
将预处理后的参考数据转化为机器人在仿真环境中的完整运动数据。注意：
最后一次帧没有后续帧用于计算速度，因此 joint_pos_data 被截去最后一帧。

（2） 输出重定向数据到文件

output_file = os.path.join(output_dir, f"{mocap_motion[0]}.txt")
output_motion(retarget_frames, output_file, mocap_motion[4], FRAME_DURATION)

保存数据：
将生成的运动数据写入指定的输出文件中，文件名根据动作名称命名。参数：
传入数据、文件路径、以及其它必要参数（例如关节顺序或数据格式信息）和帧间隔。

4. 可视化运动过程

如果配置允许可视化（config.VISUALIZE_RETARGETING 为 True），执行以下步骤：
（1）创建调试标记

num_markers = joint_pos_data.shape[1]
marker_ids = build_markers(num_markers)
foot_line_unique_ids = [None] * 4
linear_vel_unique_id = None
angular_vel_unique_id = None

build_markers：
根据关键点数量创建用于显示关键点位置的 marker 对象。foot_line_unique_ids：
用于存储足端调试线条的 ID，便于后续更新替换。linear_vel_unique_id = No
angular_vel_unique_id = None
分别初始化用于标记线性速度和角速度调试标记的变量。

（2） 循环显示每一帧

f = 0
num_frames = joint_pos_data.shape[0]
for repeat in range(1 * num_frames):time_start = time.time()f_idx = f % num_framesprint("Frame {:d}".format(f_idx))ref_joint_pos = joint_pos_data[f_idx]pose = retarget_frames[f_idx]set_pose(robot, pose)set_maker_pos(ref_joint_pos, marker_ids)foot_line_unique_ids = set_foot_marker_pos(get_tar_toe_pos_local(pose), robot, foot_line_unique_ids)linear_vel_unique_id = set_linear_vel_pos(get_linear_vel(pose), robot, linear_vel_unique_id)angular_vel_unique_id = set_angular_vel_pos(get_angular_vel(pose), robot, angular_vel_unique_id)update_camera(robot)p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING, 1)f += 1time_end = time.time()sleep_dur = FRAME_DURATION - (time_end - time_start)sleep_dur = max(0, sleep_dur)time.sleep(sleep_dur)

循环逻辑：
遍历所有帧，对每一帧：根据当前帧索引 f_idx 获取参考数据与重定向得到的 pose；使用 set_pose 更新机器人状态；更新各个 marker 显示当前关键点位置（set_maker_pos）、足端目标位置（set_foot_marker_pos），以及线性和角速度显示（set_linear_vel_pos、set_angular_vel_pos）；调用 update_camera 保持摄像机跟随机器人；计算处理时间并通过 time.sleep 保持帧率一致。

（3）清除调试对象

for m in marker_ids:p.removeBody(m)
p.removeAllUserDebugItems()
marker_ids = []

清理工作：
结束可视化后，移除所有创建的 marker 和调试对象，防止资源泄露。

5. 结束仿真与退出

pybullet.disconnect()
return

断开连接：
调用 pybullet.disconnect() 断开与物理仿真服务器的连接，结束整个仿真过程。返回：
函数结束，不返回值。

1. 启动与环境配置

1.1 启动 Omniverse 应用

命令行参数设置：

利用 argparse 构造命令行参数解析器，并调用 AppLauncher.add_app_launcher_args(parser) 添加 Omniverse 应用需要的参数。

parser = argparse.ArgumentParser()
AppLauncher.add_app_launcher_args(parser)
args_cli = parser.parse_args()
args_cli.headless = True

将 args_cli.headless 设置为 True 意味着以无头模式运行，适用于无需显示 GUI 的场景。

启动应用：
创建 AppLauncher 实例并启动 Omniverse 应用，将返回的应用对象存储在 simulation_app 中，后续仿真可能依赖该环境。

app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app

1.2 模块导入与常量定义

导入库：

（1）标准库： 包括 os, sys, time, inspect 等，提供文件操作、系统接口和时间处理功能。
（2）第三方库： 如 numpy 用于数值计算，pyquaternion 处理四元数，pybullet 用于物理仿真，以及 pybullet_data 提供 URDF 等资源。
（3）自定义模块：

retarget_utils 中包含一些工具函数（例如设置机器人姿态、更新调试信息等）；
retarget_config 提供各类配置参数，如 URDF 文件路径、初始位姿、偏移量、关节 damping、动画输出目录等。
build_serial_chain_from_urdf 用于构建机器人末端执行器（如脚）的运动学链条；
rsl_rl_amp.datasets.pose3d 提供与 3D 姿态数据相关的操作，比如旋转点的函数。

参数定义

2. 机器人模型与运动链构建

2.1 运动链构建

构建足端链条：
利用 build_serial_chain_from_urdf 从配置文件中读取的 URDF 数据构建机器人四个脚（左前、右前、左后、右后）的运动链。这些链条对象后续用于通过正向运动学计算各脚末端在局部坐标系中的位置。

chain_foot_fl = build_serial_chain_from_urdf(open(config.URDF_FILENAME, 'rb').read(), config.FL_FOOT_NAME
)
chain_foot_fr = build_serial_chain_from_urdf(open(config.URDF_FILENAME, 'rb').read(), config.FR_FOOT_NAME
)
chain_foot_rl = build_serial_chain_from_urdf(open(config.URDF_FILENAME, 'rb').read(), config.HL_FOOT_NAME
)
chain_foot_rr = build_serial_chain_from_urdf(open(config.URDF_FILENAME, 'rb').read(), config.HR_FOOT_NAME
)

（1）代码功能

目标：从URDF文件中解析四条腿的运动学链（基座 → 髋关节 → 膝关节 → 脚部）。方法：使用 build_serial_chain_from_urdf 函数，基于URDF文件定义和每条腿的末端链接名称，构建独立的运动链。

（2）关键组件解析
build_serial_chain_from_urdf 函数

作用：从URDF文件中提取从基座到指定末端链接的关节序列，构建运动学链。

输入参数：

    urdf_content：URDF文件内容（字符串格式）。end_effector_name：末端链接名称（如脚部链接）。输出：一个包含关节顺序和运动学计算方法的链对象。

3. 数据提取与设置函数

3.1 数据提取函数

为了方便后续对 pose 数组中各部分数据的提取，定义了几个函数：

def get_joint_pose(pose):return pose[(POS_SIZE + ROT_SIZE) : (POS_SIZE + ROT_SIZE + JOINT_POS_SIZE)]def get_tar_toe_pos_local(pose):return pose[(POS_SIZE + ROT_SIZE + JOINT_POS_SIZE) : (POS_SIZE + ROT_SIZE + JOINT_POS_SIZE + TAR_TOE_POS_LOCAL_SIZE)]def get_linear_vel(pose):return pose[(POS_SIZE + ROT_SIZE + JOINT_POS_SIZE + TAR_TOE_POS_LOCAL_SIZE) : (POS_SIZE+ ROT_SIZE+ JOINT_POS_SIZE+ TAR_TOE_POS_LOCAL_SIZE+ LINEAR_VEL_SIZE)]def get_angular_vel(pose):return pose[(POS_SIZE+ ROT_SIZE+ JOINT_POS_SIZE+ TAR_TOE_POS_LOCAL_SIZE+ LINEAR_VEL_SIZE) : (POS_SIZE+ ROT_SIZE+ JOINT_POS_SIZE+ TAR_TOE_POS_LOCAL_SIZE+ LINEAR_VEL_SIZE+ ANGULAR_VEL_SIZE)]def get_joint_vel(pose):return pose[(POS_SIZE+ ROT_SIZE+ JOINT_POS_SIZE+ TAR_TOE_POS_LOCAL_SIZE+ LINEAR_VEL_SIZE+ ANGULAR_VEL_SIZE) : (POS_SIZE+ ROT_SIZE+ JOINT_POS_SIZE+ TAR_TOE_POS_LOCAL_SIZE+ LINEAR_VEL_SIZE+ ANGULAR_VEL_SIZE+ JOINT_VEL_SIZE)]def get_tar_toe_vel_local(pose):return pose[(POS_SIZE+ ROT_SIZE+ JOINT_POS_SIZE+ TAR_TOE_POS_LOCAL_SIZE+ LINEAR_VEL_SIZE+ ANGULAR_VEL_SIZE+ JOINT_VEL_SIZE) : (POS_SIZE+ ROT_SIZE+ JOINT_POS_SIZE+ TAR_TOE_POS_LOCAL_SIZE+ LINEAR_VEL_SIZE+ ANGULAR_VEL_SIZE+ JOINT_VEL_SIZE+ TAR_TOE_VEL_LOCAL_SIZE)]

典型顺序：位置→旋转→关节角度→脚部位置→线速度→角速度→关节速度→脚部速度

# 假设从文件加载一帧数据
pose_frame = np.loadtxt("motion_frame.txt")# 分解数据
root_pos = pose_frame[0:3]                   # 直接切片
root_rot = pose_frame[3:7]                   # 直接切片
joint_angles = get_joint_pose(pose_frame)    # 使用工具函数
foot_vel = get_tar_toe_vel_local(pose_frame) # 获取脚部速度# 应用示例：发送关节角度到控制器
robot.set_joint_positions(joint_angles)

3.2 数据设置函数

def set_root_pos(root_pos, pose):pose[0:POS_SIZE] = root_posreturndef set_root_rot(root_rot, pose):pose[POS_SIZE : (POS_SIZE + ROT_SIZE)] = root_rotreturndef set_joint_pose(joint_pose, pose):pose[(POS_SIZE + ROT_SIZE) :] = joint_posereturndef set_maker_pos(marker_pos, marker_ids):num_markers = len(marker_ids)assert num_markers == marker_pos.shape[0]for i in range(num_markers):curr_id = marker_ids[i]curr_pos = marker_pos[i]pybullet.resetBasePositionAndOrientation(curr_id, curr_pos, DEFAULT_ROT)returndef set_foot_marker_pos(marker_pos, robot_idx, unique_ids=None):marker_pos = marker_pos.reshape(4, 3)new_unique_ids = []for foot_pos, unique_id in zip(marker_pos, unique_ids):if unique_id is not None:new_unique_ids.append(pybullet.addUserDebugLine(lineFromXYZ=foot_pos - np.array([0.0, 0.0, 0.04]),lineToXYZ=foot_pos + np.array([0.0, 0.0, 0.04]),lineWidth=4,replaceItemUniqueId=unique_id,lineColorRGB=[1, 0, 0],parentObjectUniqueId=robot_idx,))else:new_unique_ids.append(pybullet.addUserDebugLine(lineFromXYZ=foot_pos - np.array([0.0, 0.0, 0.04]),lineToXYZ=foot_pos + np.array([0.0, 0.0, 0.04]),lineWidth=4,lineColorRGB=[1, 0, 0],parentObjectUniqueId=robot_idx,))return new_unique_ids

前三个简单不介绍了，

参考标记更新 set_maker_pos

def set_maker_pos(marker_pos, marker_ids):for i in range(len(marker_ids)):pybullet.resetBasePositionAndOrientation(marker_ids[i], marker_pos[i], DEFAULT_ROT)

作用：批量更新参考骨架标记的位置参数：marker_pos: (N,3) 数组，N个标记的位置marker_ids: 预先创建的标记物体ID列表可视化效果：使球形标记跟随参考运动数据移动

脚部指示线更新 set_foot_marker_pos

def set_foot_marker_pos(marker_pos, robot_idx, unique_ids=None):for foot_pos in marker_pos:pybullet.addUserDebugLine(foot_pos - [0,0,0.04], foot_pos + [0,0,0.04],lineColorRGB=[1,0,0],replaceItemUniqueId=unique_id,parentObjectUniqueId=robot_idx)

作用：在每只脚位置显示垂直红线（高度指示）参数：marker_pos: (4,3) 数组，四足末端位置unique_ids: 之前创建的线条ID（用于更新而非新建）设计亮点：replaceItemUniqueId 实现高效更新，避免内存泄漏parentObjectUniqueId 使线条随机器人移动

3.3 可视化辅助函数

def build_markers(num_markers):

代码逐行解析

（1）初始化参数

marker_radius = 0.02  # 标记的半径（单位：米）
markers = []          # 存储所有标记的ID列表

（2）循环创建标记

for i in range(num_markers):# 根据关节类型选择颜色if (i == REF_NECK_JOINT_ID) or (i == REF_PELVIS_JOINT_ID) or (i in REF_HIP_JOINT_IDS):col = [0, 0, 1, 1]  # 蓝色：关键关节（颈部、骨盆、髋部）elif i in REF_TOE_JOINT_IDS:col = [1, 0, 0, 1]  # 红色：脚趾关节else:col = [0, 1, 0, 1]  # 绿色：其他关节

（3）创建视觉形状

virtual_shape_id = pybullet.createVisualShape(shapeType=pybullet.GEOM_SPHERE,  # 形状为球体radius=marker_radius,             # 半径0.02米rgbaColor=col                     # 颜色根据关节类型设置
)

作用：定义一个球形的视觉形状，无物理碰撞属性。

（4）创建多体对象

body_id = pybullet.createMultiBody(baseMass=0,                     # 质量为0（静态物体，不受重力影响）baseCollisionShapeIndex=-1,     # 无碰撞形状（不会与其他物体碰撞）baseVisualShapeIndex=virtual_shape_id,  # 关联视觉形状basePosition=[0, 0, 0],         # 初始位置（后续通过`set_maker_pos`更新）useMaximalCoordinates=True      # 使用最大坐标模式（优化静态物体性能）
)
markers.append(body_id)  # 将标记ID加入列表

关键参数：

 baseMass=0：静态物体，固定不动。useMaximalCoordinates=True：提高计算效率，适用于固定位置的物体。

（5）返回结果

return markers  # 返回所有标记的ID列表

4. 数据预处理与坐标变换

4.1 参考关节位置预处理

功能
将原始运动捕捉数据转换到仿真环境坐标系，并进行缩放和偏移调整。

def process_ref_joint_pos_data(joint_pos):proc_pos = joint_pos.copy()num_pos = joint_pos.shape[0]for i in range(num_pos):curr_pos = proc_pos[i]# 坐标系旋转变换curr_pos = pose3d.QuaternionRotatePoint(curr_pos, REF_COORD_ROT)  # Y-up → Z-upcurr_pos = pose3d.QuaternionRotatePoint(curr_pos, REF_ROOT_ROT)   # 初始朝向调整# 缩放与偏移curr_pos = curr_pos * config.REF_POS_SCALE + REF_POS_OFFSETproc_pos[i] = curr_posreturn proc_pos

4.2 根关节位姿计算

功能

根据骨盆和肩部位置计算机器人基座（Root）的位置和朝向

def retarget_root_pose(ref_joint_pos):pelvis_pos = ref_joint_pos[REF_PELVIS_JOINT_ID]neck_pos = ref_joint_pos[REF_NECK_JOINT_ID]left_shoulder_pos = ref_joint_pos[REF_HIP_JOINT_IDS[0]]right_shoulder_pos = ref_joint_pos[REF_HIP_JOINT_IDS[1]]left_hip_pos = ref_joint_pos[REF_HIP_JOINT_IDS[2]]right_hip_pos = ref_joint_pos[REF_HIP_JOINT_IDS[3]]forward_dir = neck_pos - pelvis_posforward_dir += config.FORWARD_DIR_OFFSETforward_dir = forward_dir / np.linalg.norm(forward_dir)delta_shoulder = left_shoulder_pos - right_shoulder_posdelta_hip = left_hip_pos - right_hip_posdir_shoulder = delta_shoulder / np.linalg.norm(delta_shoulder)dir_hip = delta_hip / np.linalg.norm(delta_hip)left_dir = 0.5 * (dir_shoulder + dir_hip)up_dir = np.cross(forward_dir, left_dir)up_dir = up_dir / np.linalg.norm(up_dir)left_dir = np.cross(up_dir, forward_dir)left_dir[2] = 0.0  # make the base more stableleft_dir = left_dir / np.linalg.norm(left_dir)rot_mat = np.array([[forward_dir[0], left_dir[0], up_dir[0], 0],[forward_dir[1], left_dir[1], up_dir[1], 0],[forward_dir[2], left_dir[2], up_dir[2], 0],[0, 0, 0, 1],])root_pos = 0.5 * (pelvis_pos + neck_pos)# root_pos = 0.25 * (left_shoulder_pos + right_shoulder_pos + left_hip_pos + right_hip_pos)root_rot = transformations.quaternion_from_matrix(rot_mat)root_rot = transformations.quaternion_multiply(root_rot, config.INIT_ROT)root_rot = root_rot / np.linalg.norm(root_rot)return root_pos, root_rot

（1）计算前向方向

forward_dir = (neck_pos - pelvis_pos) + config.FORWARD_DIR_OFFSET
forward_dir /= np.linalg.norm(forward_dir)

neck_pos - pelvis_pos：
这部分计算了脖子位置与骨盆位置之间的向量，作为身体的主要前向参考。通常脖子位于身体前部，所以这个向量近似表示身体的朝向。+ config.FORWARD_DIR_OFFSET：
根据配置文件中的偏移值对前向向量进行微调，可能是为了补偿人体姿态或摄像头标定等实际问题，使得前向方向更符合预期。归一化：
使用 np.linalg.norm(forward_dir) 将向量长度归一化，得到一个单位向量，保证后续计算时方向信息准确无误。

（2）计算左右方向

left_dir = 0.5*(normalize(left_shoulder - right_shoulder) + normalize(left_hip - right_hip))

normalize(left_shoulder - right_shoulder)：
计算左右肩（或上半身）的水平方向。这里用左肩减去右肩得到一个从右向左的向量，并对其归一化。normalize(left_hip - right_hip)：
同样，计算左右髋（下半身）的水平方向。得到的向量表示从右向左的方向，也归一化。加权平均：
对肩部和髋部的左右方向取平均（0.5 倍），这样可以综合上半身和下半身的方向信息，得到更加稳健的整体左侧方向。

（3）计算上方向

up_dir = np.cross(forward_dir, left_dir)
up_dir /= np.linalg.norm(up_dir)

叉乘计算上方向：
使用 np.cross(forward_dir, left_dir) 计算前向和左向的叉乘。叉乘结果得到的向量垂直于这两个向量，即近似为身体的竖直向上方向。归一化：
同样归一化得到单位向量，确保上方向的长度为 1，便于后续构造旋转矩阵时直接使用。

（4）修正左右方向

left_dir = np.cross(up_dir, forward_dir)
left_dir[2] = 0  # 消除Z轴分量
left_dir /= np.linalg.norm(left_dir)

二次修正：
再次利用叉乘计算：np.cross(up_dir, forward_dir) 得到一个新的左右方向向量。这里的目的在于确保这个向量和上方向、前向方向严格正交。水平面投影：
将左右方向向量的 Z 分量置为 0（left_dir[2] = 0），意味着强制将该向量投影到水平面上。这通常是为了保证旋转矩阵中的“左”方向不含竖直方向的成分，从而使机器人的“水平”姿态更加稳定。归一化：
最后对修正后的左右向量进行归一化，确保它为单位向量。

（5） 构建旋转矩阵

rot_mat = [[forward[0], left[0], up[0], 0],[forward[1], left[1], up[1], 0],[forward[2], left[2], up[2], 0],[0, 0, 0, 1]]

矩阵构造：
构造的旋转矩阵 rot_mat 是一个 4×4 的齐次变换矩阵。第一列：前向方向 forward第二列：修正后的左向 left第三列：上方向 up第四列：平移部分，此处为零向量（最后一行 [0,0,0,1] 表示齐次坐标系）这个矩阵定义了从局部坐标系到全局坐标系的旋转变换，也可以用来表示机器人的朝向。

注意： 代码中实际构造矩阵时用了变量名 forward, left, up，这要求前面计算的方向变量名和这里一致，通常在实际代码中要确保变量名称统一（比如上面的 forward_dir、left_dir、up_dir）。

（6）计算根位置

root_pos = 0.5*(pelvis_pos + neck_pos)

根位置被定义为骨盆位置 pelvis_pos 与脖子位置 neck_pos 的平均值。
这样计算得到的中点大致位于人体的中部，作为机器人整个身体的基准位置。

（7）计算根的旋转

从旋转矩阵转换为四元数：

root_rot = transformations.quaternion_from_matrix(rot_mat)

这一行调用 transformations.quaternion_from_matrix 函数，将之前构造好的 4×4 旋转矩阵 rot_mat 转换为四元数。四元数是一种常用的表示旋转的方式，相比欧拉角可以避免万向节锁问题，并且插值时更平滑。

与初始旋转组合：

root_rot = transformations.quaternion_multiply(root_rot, config.INIT_ROT)

这一步通过 transformations.quaternion_multiply 将从矩阵中获得的四元数 root_rot 与配置中的初始旋转 config.INIT_ROT 相乘。
四元数乘法用于组合旋转，乘积的顺序决定了先后应用哪一个旋转。这里表示先应用从矩阵得到的旋转，再应用初始旋转，从而确保最终的姿态与整个系统的初始对齐保持一致。

归一化四元数：

root_rot = root_rot / np.linalg.norm(root_rot)

作用：
归一化操作确保四元数的模长为1，即得到一个单位四元数。单位四元数是表示旋转的标准形式，它能避免数值误差带来的偏差，确保旋转计算的稳定性。

5. 重定向核心流程

5.1 逆运动学求解

retarget_pose 的主要目的是根据参考的关节数据 ref_joint_pos，计算出机器人当前的完整姿态（pose），包括根节点（位置和旋转）、关节角度以及目标足端（toe）在局部坐标系下的位置。该函数通常用于将动作捕捉数据或关键点数据转化为机器人可执行的状态，然后利用逆运动学（IK）求解各个关节的角度。
（1）获取关节限制

joint_lim_low, joint_lim_high = get_joint_limits(robot)
joint_lim_low = [i * -1 for i in joint_lim_high]
joint_lim_high = [i * -1 for i in joint_lim_high]

说明：
首先调用 get_joint_limits(robot) 获取机器人的关节限制，返回下限和上限。随后代码对上下限做了取负操作（即将每个值乘以 -1）。这种处理方式可能与机器人模型的定义有关，用于调整逆运动学求解时的限制范围，确保求解的角度与模型实际对应。

（2）计算根节点（root）的位姿

root_pos, root_rot = retarget_root_pose(ref_joint_pos)
root_pos += config.SIM_ROOT_OFFSET

retarget_root_pose(ref_joint_pos)：
根据参考关键点（例如骨盆和脖子）计算得到根位置和根旋转。通常根位置取骨盆和脖子中点，旋转则通过计算前向、侧向和上向量构成旋转矩阵再转换为四元数。添加偏移：
root_pos += config.SIM_ROOT_OFFSET 用于将计算出的根位置再加上一个仿真配置中的偏移量，这样可以根据场景需求对机器人整体位置做调整。

（3）更新机器人基础状态

pybullet.resetBasePositionAndOrientation(robot, root_pos, root_rot)

使用 PyBullet 的接口 resetBasePositionAndOrientation 将机器人模型的根节点位置和旋转更新为刚刚计算的 root_pos 和 root_rot，使得机器人在仿真环境中处于正确的初始状态。

（4）计算 Heading 旋转

inv_init_rot = transformations.quaternion_inverse(config.INIT_ROT)
heading_rot = calc_heading_rot(transformations.quaternion_multiply(root_rot, inv_init_rot)
)

逆初始旋转：
先计算 config.INIT_ROT 的逆，即 inv_init_rot。这样做的目的是将当前旋转 root_rot 与初始状态对齐。四元数乘法：
通过 transformations.quaternion_multiply(root_rot, inv_init_rot) 组合旋转，得到相对于初始旋转的旋转差。计算 Heading：
将这个旋转差传入 calc_heading_rot（这是自定义函数，用于提取出水平方向的旋转或偏航角），得到 heading 旋转。这一步用于将局部偏移转换到世界坐标系下正确的方向。

（5）计算目标足端位置（tar_toe_pos）

这一段代码遍历所有的足端（参考索引在 REF_TOE_JOINT_IDS 中），依次计算每个足端的目标位置：

tar_toe_pos = []
for i in range(len(REF_TOE_JOINT_IDS)):ref_toe_id = REF_TOE_JOINT_IDS[i]ref_hip_id = REF_HIP_JOINT_IDS[i]sim_hip_id = config.SIM_HIP_JOINT_IDS[i]toe_offset_local = config.SIM_TOE_OFFSET_LOCAL[i]ref_toe_pos = ref_joint_pos[ref_toe_id]ref_hip_pos = ref_joint_pos[ref_hip_id]hip_link_state = pybullet.getLinkState(robot, sim_hip_id, computeForwardKinematics=True)sim_hip_pos = np.array(hip_link_state[4])toe_offset_world = pose3d.QuaternionRotatePoint(toe_offset_local, heading_rot)ref_hip_toe_delta = ref_toe_pos - ref_hip_possim_tar_toe_pos = sim_hip_pos + ref_hip_toe_deltasim_tar_toe_pos[2] = ref_toe_pos[2]sim_tar_toe_pos += toe_offset_worldtar_toe_pos.append(sim_tar_toe_pos)

• 初始化目标足端位置列表

tar_toe_pos = []

创建一个空列表，用于保存所有计算得到的足端目标位置。

• 遍历每个足端

for i in range(len(REF_TOE_JOINT_IDS)):

循环遍历每个足端。REF_TOE_JOINT_IDS 存储了参考数据中各个足端的关键点索引，因此循环次数即为足端数量。

• 获取参考数据索引和配置参数

ref_toe_id = REF_TOE_JOINT_IDS[i]
ref_hip_id = REF_HIP_JOINT_IDS[i]
sim_hip_id = config.SIM_HIP_JOINT_IDS[i]
toe_offset_local = config.SIM_TOE_OFFSET_LOCAL[i]

ref_toe_id 与 ref_hip_id：
分别从参考数据中获取当前足端和对应臀部（hip）的索引。sim_hip_id：
从配置中获取仿真机器人中对应臀部的链接 ID，用于查询当前仿真状态下的臀部位置。toe_offset_local：
获取预设的局部足端偏移，该偏移值通常用于对齐实际足端和参考数据之间的差异（例如，由于模型结构或比例问题）。

• 提取参考关键点数据

ref_toe_pos = ref_joint_pos[ref_toe_id]
ref_hip_pos = ref_joint_pos[ref_hip_id]

根据索引从参考关节数据 ref_joint_pos 中提取出足端和臀部的位置。这些位置是采集或预处理后的参考数据。

• 获取机器人当前臀部位置

hip_link_state = pybullet.getLinkState(robot, sim_hip_id, computeForwardKinematics=True)
sim_hip_pos = np.array(hip_link_state[4])

调用 pybullet.getLinkState 获取机器人当前状态下指定臀部链接的信息，其中参数 computeForwardKinematics=True 表示要求计算前向运动学。hip_link_state[4] 通常返回世界坐标系下的链接位置。将其转换为 NumPy 数组，得到当前仿真中的臀部位置 sim_hip_pos。

• 转换局部偏移到世界坐标系

toe_offset_world = pose3d.QuaternionRotatePoint(toe_offset_local, heading_rot)

利用 pose3d.QuaternionRotatePoint 函数，将局部足端偏移 toe_offset_local 按照 heading 旋转（计算得到的旋转角度），转换到世界坐标系中。这样得到的 toe_offset_world 表示在世界坐标系下足端应有的偏移量。

• 计算参考中臀部到足端的相对偏移

ref_hip_toe_delta = ref_toe_pos - ref_hip_pos

计算参考数据中，臀部到足端之间的向量差。这个向量描述了在参考动作中，足端相对于臀部的相对位置。

• 得到初步目标足端位置

sim_tar_toe_pos = sim_hip_pos + ref_hip_toe_delta

将参考中计算得到的相对偏移 ref_hip_toe_delta 加到当前仿真中实际的臀部   位置 sim_hip_pos 上，得到一个初步的目标足端位置 sim_tar_toe_pos。这一步的目的是将参考数据的相对位置信息“映射”到当前机器人的实际位置上。

• 调整 Z 轴（高度）

sim_tar_toe_pos[2] = ref_toe_pos[2]

将目标足端位置的 Z 轴分量（高度）设为参考数据中足端位置的 Z 值。这样做通常是为了确保足端在高度上的定位与参考数据一致，防止因机器人模型或计算误差导致足端高度偏差。

• 添加局部偏移

sim_tar_toe_pos += toe_offset_world

在之前计算得到的目标足端位置上加上转换后的局部偏移 toe_offset_world，对位置进行微调，使得最终目标位置更加符合预期。

• 保存目标位置

tar_toe_pos.append(sim_tar_toe_pos)

将计算出的当前足端目标位置添加到 tar_toe_pos 列表中，供后续逆运动学求解时使用。

---

（6）逆运动学求解得到关节角度

joint_pose = pybullet.calculateInverseKinematics2(robot,config.SIM_TOE_JOINT_IDS,tar_toe_pos,jointDamping=config.JOINT_DAMPING,lowerLimits=joint_lim_low,upperLimits=joint_lim_high,restPoses=default_pose,
)
joint_pose = np.array(joint_pose)

IK 求解：
使用 PyBullet 的 calculateInverseKinematics2 接口，根据目标足端位置 tar_toe_pos 和给定的关节限制、关节 damping 以及默认姿态（作为初始猜测），求解得到机器人各个关节的角度值。

robot： 表示当前仿真中加载的机器人模型的 ID。

config.SIM_TOE_JOINT_IDS： 是一个列表，指定了机器人中对应足端的关节（或链接）的ID，逆运动学求解时会使这些末端执行器尽量到达目标位置。

tar_toe_pos： 列表形式的目标位置，每个元素是对应足端在世界坐标系下的 3D 坐标，由之前的计算得出。

jointDamping=config.JOINT_DAMPING： 用于平滑求解的关节阻尼参数，较高的阻尼有助于避免震荡，提升解的稳定性。

lowerLimits=joint_lim_low, upperLimits=joint_lim_high：分别指定每个关节允许运动的下限和上限，确保求解的角度不会超出机械结构的安全范围。

restPoses=default_pose： 提供一个默认关节姿态作为求解的初始猜测，有助于逆运动学算法更快收敛到一个合理的解。

转换为数组：
将返回的结果转换为 NumPy 数组，方便后续数据处理。

（7） 利用正向运动学计算局部足端位置

tar_toe_pos_local = np.squeeze(np.concatenate([chain_foot_fl.forward_kinematics(joint_pose[:3]).get_matrix()[:, :3, 3],chain_foot_fr.forward_kinematics(joint_pose[3:6]).get_matrix()[:, :3, 3],chain_foot_rl.forward_kinematics(joint_pose[6:9]).get_matrix()[:, :3, 3],chain_foot_rr.forward_kinematics(joint_pose[9:12]).get_matrix()[:, :3, 3],],axis=-1,)
)pose = np.concatenate([root_pos, root_rot, joint_pose, tar_toe_pos_local])

• 正运动学计算

  输入：各腿的关节角度

   joint_pose[:3]：前左腿的3个关节角度joint_pose[3:6]：前右腿的3个关节角度joint_pose[6:9]：后左腿的3个关节角度joint_pose[9:12]：后右腿的3个关节角度
  

  方法：
  每个 chain_foot_xx.forward_kinematics() 根据关节角度计算机体到脚部的变换矩阵。

• 位置提取

  变换矩阵结构：
  一个4x4齐次变换矩阵，格式为：

其中 R 为旋转矩阵，t 为平移向量（位置）。索引操作：
get_matrix()[:, :3, 3] 实际应为 get_matrix()[:3, 3]，提取前三行第四列的位置向量 [x, y, z]。
（注：原代码中的 [:, :3, 3] 可能存在冗余维度，需根据实际 get_matrix() 的返回值调整）

• 数据拼接

  np.concatenate(…, axis=-1)：
  将四个脚部的位置向量沿 列方向（最后一个轴） 拼接，得到一个形状为 (3, 4) 的二维数组（假设每个位置是3元素向量）。

• 维度压缩

  np.squeeze()：
  移除长度为1的维度。例如，若拼接后为 (3, 4)，则无需压缩；若存在不必要的维度如 (1, 3, 4)，则压缩为 (3, 4)。

---

（8）拼接所有数据生成最终的 pose

pose = np.concatenate([root_pos, root_rot, joint_pose, tar_toe_pos_local])

作用：
将之前计算的各部分数据依次拼接：根节点位置 root_pos根节点旋转（四元数）root_rot关节角度 joint_pose局部足端位置 tar_toe_pos_local得到一个完整的状态向量，表示当前帧机器人整体的姿态信息。返回结果：
最后返回这个拼接好的 pose 作为本帧的运动状态，供后续使用或保存。

5.2 运动序列重定向

（1） update_camera(robot)

该函数用于更新调试视角，使得摄像机始终跟随机器人基座的位置。具体流程如下：

获取机器人基座位置：

base_pos = np.array(pybullet.getBasePositionAndOrientation(robot)[0])

通过 pybullet.getBasePositionAndOrientation(robot) 获取机器人当前基座的位置和朝向。[0] 表示只取返回的第一个元素，即机器人的位置（一个三维坐标）。使用 np.array() 将位置转换为 NumPy 数组，方便后续计算或传递给其他函数。

获取当前调试摄像机参数：

[yaw, pitch, dist] = pybullet.getDebugVisualizerCamera()[8:11]

调用 pybullet.getDebugVisualizerCamera() 获取当前调试摄像机的状态信息，这个函数返回一个包含多个参数的列表。[8:11] 切片取出第 8 到 10 个参数，分别表示摄像机的偏航角（yaw）、俯仰角（pitch）以及与目标的距离（dist）。

重置调试摄像机位置：

pybullet.resetDebugVisualizerCamera(dist, yaw, pitch, base_pos)

调用 pybullet.resetDebugVisualizerCamera 函数，传入之前获取的距离、yaw、pitch 以及新的目标位置 base_pos。这一步使摄像机重置后始终围绕机器人基座进行观察，从而实现摄像机跟随机器人的效果。

返回：

return

 函数没有返回值，只是更新了调试摄像机的状态。

（2） load_ref_data(JOINT_POS_FILENAME, FRAME_START, FRAME_END)

该函数用于加载参考的关节位置数据（通常存储在 CSV 或 TXT 文件中），并根据提供的起始帧和结束帧对数据进行裁剪。具体流程如下：

加载数据文件：

joint_pos_data = np.loadtxt(JOINT_POS_FILENAME, delimiter=",")

使用 np.loadtxt 从文件 JOINT_POS_FILENAME 中加载数据，文件中各个数据项之间用逗号 , 分隔。加载后得到的 joint_pos_data 通常是一个二维 NumPy 数组，其中每一行对应一帧数据，每列为一个数据值。

确定起始与结束帧：

start_frame = 0 if (FRAME_START is None) else FRAME_START
end_frame = joint_pos_data.shape[0] if (FRAME_END is None) else FRAME_END

如果 FRAME_START 为 None，则默认从第 0 帧开始；否则使用提供的 FRAME_START 作为起始帧索引。同理，如果 FRAME_END 为 None，则默认裁剪到数据的最后一帧（joint_pos_data.shape[0] 表示数据的行数）；否则使用提供的 FRAME_END 作为结束帧索引。

裁剪数据：

joint_pos_data = joint_pos_data[start_frame:end_frame]

根据确定的起始帧和结束帧对数据进行切片，保留这部分数据。

返回裁剪后的数据：

return joint_pos_data

（3） retarget_motion(robot, joint_pos_data):

函数 retarget_motion(robot, joint_pos_data) 的主要作用是将参考运动数据（关节位置信息）转换为机器人在仿真环境中的运动帧数据。
它对每一帧的参考数据计算出相应的机器人状态（pose），同时计算出线性速度、角速度、关节速度和足端局部速度，最终将所有这些信息拼接成一帧完整的数据。最后，所有帧数据存储在 new_frames 数组中返回。

详细解析

• 初始化

num_frames = joint_pos_data.shape[0]
time_between_frames = FRAME_DURATION

num_frames：
得到运动数据的总帧数，假设 joint_pos_data 的每一行代表一帧的参考数据。time_between_frames：
每帧之间的时间间隔，通常从全局常量 FRAME_DURATION（例如 0.01677 秒）获得，用于速度计算。

• 遍历每一帧（除最后一帧）

for f in range(num_frames - 1):

循环遍历每一帧（除了最后一帧，因为需要与下一帧做速度差分）。

• 当前帧与下一帧的姿态计算

当前帧

ref_joint_pos = joint_pos_data[f]
curr_pose = retarget_pose(robot, config.DEFAULT_JOINT_POSE, ref_joint_pos)
set_pose(robot, curr_pose)

从 joint_pos_data 中取出第 f 帧的参考关节数据 ref_joint_pos。调用 retarget_pose 函数，将参考数据转化为机器人当前的完整状态（包括根位置、根旋转、关节角度和局部足端位置）。调用 set_pose 更新机器人状态到仿真环境中（实际调用 PyBullet 接口）。

下一帧

next_ref_joint_pos = joint_pos_data[f + 1]
next_pose = retarget_pose(robot, config.DEFAULT_JOINT_POSE, next_ref_joint_pos)

对下一帧同样计算出机器人状态 next_pose，用于后续速度计算。

• 初始化存储帧数据

if f == 0:pose_size = (curr_pose.shape[-1]+ LINEAR_VEL_SIZE+ ANGULAR_VEL_SIZE+ JOINT_POS_SIZE+ TAR_TOE_VEL_LOCAL_SIZE)new_frames = np.zeros([num_frames - 1, pose_size])

当处理第一帧时，计算单帧数据的总长度 pose_size。
这个长度包含：当前帧的状态数据（curr_pose，已经包括了根位置、根旋转、关节角度和局部足端位置）。线性速度（LINEAR_VEL_SIZE）。角速度（ANGULAR_VEL_SIZE）。关节角速度（JOINT_POS_SIZE，与关节角度数量一致）。局部足端速度（TAR_TOE_VEL_LOCAL_SIZE）。然后创建一个形状为 [num_frames - 1, pose_size] 的数组 new_frames 用于保存每一帧的完整数据。

• 速度计算

计算线性速度

del_linear_vel = (np.array((get_root_pos(next_pose) - get_root_pos(curr_pose)))/ time_between_frames
)
r = pybullet.getMatrixFromQuaternion(get_root_rot(curr_pose))
del_linear_vel = np.matmul(del_linear_vel, np.array(r).reshape(3, 3))

位置差：
通过 get_root_pos 提取当前帧与下一帧的根节点位置，相减得到位移差，再除以时间间隔得到线性速度（在全局坐标系下）。坐标转换：
通过 pybullet.getMatrixFromQuaternion 获取当前帧根旋转对应的旋转矩阵，然后将速度从全局坐标系转换到机器人基座坐标系（通过矩阵乘法）。

计算角速度

curr_quat = get_root_rot(curr_pose)
next_quat = get_root_rot(next_pose)
diff_quat = Quaternion.distance(Quaternion(curr_quat[3], curr_quat[0], curr_quat[1], curr_quat[2]),Quaternion(next_quat[3], next_quat[0], next_quat[1], next_quat[2]),
)
del_angular_vel = pybullet.getDifferenceQuaternion(get_root_rot(curr_pose), get_root_rot(next_pose)
)
axis, _ = pybullet.getAxisAngleFromQuaternion(del_angular_vel)
del_angular_vel = np.array(axis) * (diff_quat * 2) / time_between_frames

四元数差异：
利用当前帧与下一帧的根旋转四元数计算旋转差，通过 Quaternion.distance 得到旋转差的大小（以弧度表示）。旋转轴与角度：
使用 pybullet.getDifferenceQuaternion 得到描述两个四元数差异的四元数，再利用 pybullet.getAxisAngleFromQuaternion 提取旋转轴。角速度计算：
最终通过将旋转轴乘以旋转角速度（这里用 diff_quat * 2，原因可能是内部实现细节）再除以时间间隔，得到角速度的矢量表示（在当前帧基座坐标系下）。

坐标系转换的后处理

inv_init_rot = transformations.quaternion_inverse(config.INIT_ROT)
_, base_orientation_quat_from_init = pybullet.multiplyTransforms(positionA=(0, 0, 0),orientationA=inv_init_rot,positionB=(0, 0, 0),orientationB=get_root_rot(curr_pose),
)
_, inverse_base_orientation = pybullet.invertTransform([0, 0, 0], base_orientation_quat_from_init
)
del_angular_vel, _ = pybullet.multiplyTransforms(positionA=(0, 0, 0),orientationA=(inverse_base_orientation),positionB=del_angular_vel,orientationB=(0, 0, 0, 1),
)

这几行用于将角速度转换到机器人基座的局部坐标系下。首先，计算当前帧根旋转与初始旋转的关系；然后通过 invertTransform 及 
multiplyTransforms 将角速度矢量变换到基座坐标系下。
这部分代码较为繁琐，主要是为了确保角速度方向与机器人运动方向一致。

计算关节角速度与局部足端速度

joint_velocity = (np.array(get_joint_pose(next_pose) - get_joint_pose(curr_pose))/ time_between_frames
)
toe_velocity = (np.array(get_tar_toe_pos_local(next_pose) - get_tar_toe_pos_local(curr_pose))/ time_between_frames
)

joint_velocity：
通过相邻帧关节角度的差值除以时间间隔，得到关节角速度。toe_velocity：
同理，通过相邻帧局部足端位置的差值除以时间间隔，得到足端局部速度。

拼接当前帧数据

curr_pose = np.concatenate([curr_pose, del_linear_vel, del_angular_vel, joint_velocity, toe_velocity]
)
new_frames[f] = curr_pose

数据拼接：
将当前帧的状态（包含根位置、根旋转、关节角度和局部足端位置）与计算得到的线性速度、角速度、关节速度和足端速度拼接成一个长向量。存储：
将拼接后的结果存入新数组 new_frames 对应的帧 f 中。

后处理

new_frames[:, 0:2] -= new_frames[0, 0:2]

这一步将所有帧的根位置（通常前两个元素代表根节点在水平面上的位置）做了平移校正，
使得第一帧的位置为零，其他帧相对于第一帧作相应调整。
这样做有助于后续数据处理和分析，确保位置数据相对统一。

返回新帧数据

return new_frames

最终返回拼接好的运动帧数据数组 new_frames，每一行代表一帧的完整状态信息。