读代码ModelBasedFootstepPlanning-IROS2024

Mighty_Crane

已于 2024-10-19 15:55:53 修改

阅读量2.1k

点赞数 34

文章标签：深度学习

于 2024-10-18 17:11:53 首次发布

本文链接：https://blog.csdn.net/weixin_40459958/article/details/142816706

版权

env_cfg

asset:
  angular_damping: 0.1 # 表示物体的角阻尼，值为0.1，表示角速度减慢较慢
  apply_humanoid_jacobian: true # 使用类人机器人雅可比矩阵进行运动学求解
  collapse_fixed_joints: false # 不合并固定关节
  default_dof_drive_mode: 3 # 自由度驱动模式为3
  disable_actuations: false # 驱动未被禁用
  disable_gravity: false # 不禁用重力
  disable_motors: false # 不禁用电机
  end_effectors:	# 末端执行器（End Effector）
  - right_foot
  - left_foot	# 将其双脚作为主要与地面交互的部件
  file: '{LEGGED_GYM_ROOT_DIR}/resources/robots/humanoid/urdf/humanoid_fixed_arms_sf_update.urdf' # 机器人模型文件路径
  flip_visual_attachments: false # 不翻转视觉附件
  foot_name: foot # 足部名称为foot
  keypoints:
  - base # 基础关键点
  rotor_inertia: # 各个关节的转子惯性
  - 0.01188
  - 0.01188
  - 0.0198
  - 0.0792
  - 0.04752
  - 0.01188
  - 0.01188
  - 0.0198
  - 0.0792
  - 0.04752
  self_collisions: 0 # 无自碰撞检测
  terminate_after_contacts_on: # 接触这些部位后终止仿真
  - base
  - right_upper_leg
  - right_lower_leg
  - left_upper_leg
  - left_lower_leg
  - right_upper_arm
  - right_lower_arm
  - right_hand
  - left_upper_arm
  - left_lower_arm
  - left_hand

commands:
  apex_height_percentage: 0.15 # 最高点占步长的百分比
  curriculum: false # 不启用课程学习
  dstep_length: 0.5 # 步长为0.5米
  dstep_width: 0.3 # 步宽为0.3米
  max_curriculum: 1.0 # 最大课程进度为1.0
  num_commands: 3 # 允许的命令数为3
  ranges:
    dstep_width:	# 步幅宽度的范围或固定值，在这里，两个相同的值（0.3）表示步幅宽度是一个固定值（0.3米），而不是范围
    - 0.3
    - 0.3
    lin_vel_x:	# 前进速度（x轴线性速度）的范围，从 -2.0 到 2.0 米/秒。这意味着机器人可以在正反两个方向上移动
    - -2.0
    - 2.0
    lin_vel_y: 2.0
    sample_period:	# 采样周期的范围
    - 35
    - 36
    yaw_vel: 0.0 # 偏航速度为0	# 偏航速度设置为 0.0，表明在这个任务中，机器人不会有旋转的偏航运动
  resampling_time: 10.0 # 重新采样时间为10秒
  sample_angle_offset: 20 # 采样角度偏移量为20度
  sample_radius_offset: 0.05 # 采样半径偏移量为0.05米
  succeed_step_angle: 10 # 成功步角为10度
  succeed_step_radius: 0.03 # 成功步半径为0.03米

control:
  actuation_scale: 1.0 # 驱动缩放为1.0
  damping: # 关节阻尼
    01_right_hip_yaw: 1.0
    02_right_hip_abad: 1.0
    03_right_hip_pitch: 1.0
    04_right_knee: 1.0
    05_right_ankle: 1.0
    06_left_hip_yaw: 1.0
    07_left_hip_abad: 1.0
    08_left_hip_pitch: 1.0
    09_left_knee: 1.0
    10_left_ankle: 1.0
  decimation: 10 # 信号采样降频
  exp_avg_decay: null # 未指定指数平均衰减
  stiffness: # 关节刚度
    01_right_hip_yaw: 30.0
    02_right_hip_abad: 30.0
    03_right_hip_pitch: 30.0
    04_right_knee: 30.0
    05_right_ankle: 30.0
    06_left_hip_yaw: 30.0
    07_left_hip_abad: 30.0
    08_left_hip_pitch: 30.0
    09_left_knee: 30.0
    10_left_ankle: 30.0

domain_rand:
  added_mass_range:
  - -1.0
  - 1.0 # 质量随机范围
  friction_range:
  - 0.5
  - 1.25 # 摩擦系数随机范围
  max_push_vel_xy: 0.5 # 最大横向推动速度为0.5米/秒
  push_interval_s: 2.5 # 推动间隔为2.5秒
  push_robots: true # 启用推机器人功能
  randomize_base_mass: true # 随机化基部质量
  randomize_friction: true # 随机化摩擦力

env:
  episode_length_s: 5 # 每个仿真周期持续5秒
  num_actuators: 10 # 机器人有10个驱动器
  num_envs: 4096 # 同时模拟4096个环境

init_state:
  ang_vel: # 初始角速度
  - 0.0
  - 0.0
  - 0.0
  default_joint_angles: # 默认关节角度
    01_right_hip_yaw: 0.0
    02_right_hip_abad: 0.1
    03_right_hip_pitch: -0.667751
    04_right_knee: 1.4087
    05_right_ankle: -0.708876
    06_left_hip_yaw: 0.0
    07_left_hip_abad: 0.1
    08_left_hip_pitch: -0.667751
    09_left_knee: 1.4087
    10_left_ankle: -0.708876
  dof_pos_range: # 各关节位置范围
    01_right_hip_yaw:
    - -0.1
    - 0.1
    02_right_hip_abad:
    - -0.1
    - 0.3
    03_right_hip_pitch:
    - -0.8
    - -0.4
    04_right_knee:
    - 1.3
    - 1.5
    05_right_ankle:
    - -0.9
    - -0.5
    06_left_hip_yaw:
    - -0.1
    - 0.1
    07_left_hip_abad:
    - -0.1
    - 0.3
    08_left_hip_pitch:
    - -0.8
    - -0.4
    09_left_knee:
    - 1.3
    - 1.5
    10_left_ankle:
    - -0.9
    - -0.5
  dof_vel_range: # 关节速度范围
    01_right_hip_yaw:
    - -0.1
    - 0.1
    02_right_hip_abad:
    - -0.1
    - 0.1
    03_right_hip_pitch:
    - -0.1
    - 0.1
    04_right_knee:
    - -0.1
    - 0.1
    05_right_ankle:
    - -0.1
    - 0.1
    06_left_hip_yaw:
    - -0.1
    - 0.1
    07_left_hip_abad:
    - -0.1
    - 0.1
    08_left_hip_pitch:
    - -0.1
    - 0.1
    09_left_knee:
    - -0.1
    - 0.1
    10_left_ankle:
    - -0.1
    - 0.1
  lin_vel: # 初始线速度为0
  - 0.0
  - 0.0
  - 0.0
  pos: # 初始位置
  - 0.0
  - 0.0
  - 0.62
  reset_mode: reset_to_basic # 重置模式为基本模式
  root_pos_range: # 位置范围
  - - 0.0
    - 0.0
  - - 0.0
    - 0.0
  - - 0.62
    - 0.62
  - - -0.3141592653589793
    - 0.3141592653589793
  - - -0.3141592653589793
    - 0.3141592653589793
  - - -0.3141592653589793
    - 0.3141592653589793
  root_vel_range: # 根速度范围
  - - -0.5
    - 0.5
  - - -0.5
    - 0.5
  - - -0.5
    - 0.5
  - - -0.5
    - 0.5
  - - -0.5
    - 0.5
  - - -0.5
    - 0.5
  rot: # 初始旋转
  - 0.0
  - 0.0
  - 0.0
  - 1.0

rewards:
  base_height_target: 0.62 # 目标基准高度
  curriculum: false # 不使用课程学习
  max_contact_force: 1500.0 # 最大接触力为1500牛顿
  only_positive_rewards: false # 允许负面奖励
  soft_dof_pos_limit: 0.9 # 软位置限制为0.9
  soft_dof_vel_limit: 0.9 # 软速度限制为0.9
  soft_torque_limit: 0.8 # 软力矩限制为0.8
  termination_weights:
    termination: 1.0 # 终止权重
  tracking_sigma: 0.25 # 跟踪标准差
  weights: # 各种权重
    actuation_rate: 0.001
    actuation_rate2: 0.0001
    ang_vel_xy: 0.01
    base_heading: 3.0
    base_height: 1.0
    base_z_orientation: 1.0
    contact_schedule: 3.0
    dof_pos_limits: 10
    dof_vel: 0.001
    joint_regularization: 1.0
    lin_vel_z: 0.1
    torque_limits: 0.01
    torques: 0.0001
    tracking_lin_vel_world: 4.0

scaling:
  base_ang_vel: 1.0 # 基础角速度缩放
  base_height: 1.0 # 基础高度缩放
  base_lin_vel: 1.0 # 基础线速度缩放
  clip_actions: 10.0 # 动作剪裁范围
  commands: 1.0 # 命令缩放
  dof_pos: 1.0 # 自由度位置缩放
  dof_pos_target: 1.0 # 自由度目标位置缩放
  dof_vel: 1.0 # 自由度速度缩放
  foot_states_left: 1.0 # 左脚状态缩放
  foot_states_right: 1.0 # 右脚状态缩放
  projected_gravity: 1.0 # 重力投影缩放

seed: -1 # 随机种子

terrain:
  curriculum: false # 不使用课程学习
  difficulty: 5.0 # 地形难度为5
  measure_heights: false # 不测量高度
  measured_points_x_num_sample: 33 # X轴测量点数为33
  measured_points_x_range: 
  - -0.8
  - 0.8 # X轴测量范围
  measured_points_y_num_sample: 33 # Y轴测量点数为33
  measured_points_y_range:
  - -0.8
  - 0.8 # Y轴测量范围
  mesh_type: plane # 地形网格类型为平面
  selected: true # 选中地形
  terrain_kwargs:
    type: stepping_stones # 地形类型为跳石
  terrain_length: 18.0 # 地形长度18米
  terrain_proportions:	# 地形的不同类型在仿真环境中的比例分布，和为1
  - 0.0
  - 0.5
  - 0.0
  - 0.5
  - 0.0
  - 0.0
  - 0.0
  terrain_width: 18.0 # 地形宽度18米

asset_options

angular_damping: 0.10000000149011612 
# 表示角速度的阻尼系数，0.1意味着在仿真中，物体的旋转会有轻微的减速效应。这是一个较小的阻尼值，表示旋转运动的衰减不明显，物体的旋转速度不会很快被抑制。

armature: 0.0 
# 用于模拟关节的转动惯量，值为0表示不添加额外的惯性。这通常用于简单的物理模型，适合不需要精确动态表现的场景。

collapse_fixed_joints: false 
# 表示不合并固定关节。该值为false意味着即使关节是固定的，也会单独存在并模拟其物理属性。

convex_decomposition_from_submeshes: false 
# 不从子网格进行凸分解。值为false表示不会对子网格进行独立的几何分解处理，保持原始网格结构。

default_dof_drive_mode: 3 
# 自由度驱动模式。值为3通常代表一种特定的控制模式（可能是位置控制或速度控制），其具体含义取决于使用的引擎实现。

density: 0.0010000000474974513 
# 物体的密度，单位是kg/m³。密度为0.001意味着物体非常轻或稀疏，通常适用于模拟空气状或海绵状的材料。这个值非常低，属于极轻物体的仿真。

disable_gravity: false 
# 表示不禁用重力。值为false意味着物体将受重力影响，符合常规物理情况。

enable_gyroscopic_forces: true 
# 启用陀螺效应。值为true表示旋转运动中会模拟陀螺力矩的影响，适用于有复杂旋转运动的物体或机器人。

fix_base_link: false 
# 表示基底链接是否固定。值为false意味着基底链接是活动的，允许物体在仿真中移动。

flip_visual_attachments: false 
# 不翻转视觉附加组件。false意味着保持视觉组件的默认方向。

linear_damping: 0.0 
# 线性阻尼系数，0.0表示没有线性阻尼，物体的线性运动不会受到衰减影响。通常，这会导致物体以恒定的速度运动，直到有外力作用。

max_angular_velocity: 1000.0 
# 允许的最大角速度，单位为rad/s。1000是一个非常高的值，适用于高速旋转的物体，不会对角速度施加限制。

max_linear_velocity: 1000.0 
# 允许的最大线性速度，单位为m/s。1000是极大的值，允许物体在仿真中以非常高的速度移动，适合高速场景或极限测试。

mesh_normal_mode:
  # 定义网格法线计算的模式
  COMPUTE_PER_FACE:
    COMPUTE_PER_FACE: visited
    COMPUTE_PER_VERTEX:
      COMPUTE_PER_FACE: visited
      COMPUTE_PER_VERTEX: visited
      FROM_ASSET:
        COMPUTE_PER_FACE: visited
        COMPUTE_PER_VERTEX: visited
        FROM_ASSET: visited
        name: FROM_ASSET
      name: COMPUTE_PER_VERTEX
    FROM_ASSET:
      COMPUTE_PER_FACE: visited
      COMPUTE_PER_VERTEX:
        COMPUTE_PER_FACE: visited
        COMPUTE_PER_VERTEX: visited
        FROM_ASSET: visited
        name: COMPUTE_PER_VERTEX
      FROM_ASSET: visited
      name: FROM_ASSET
    name: COMPUTE_PER_FACE
  COMPUTE_PER_VERTEX:
    COMPUTE_PER_FACE:
      COMPUTE_PER_FACE: visited
      COMPUTE_PER_VERTEX: visited
      FROM_ASSET:
        COMPUTE_PER_FACE: visited
        COMPUTE_PER_VERTEX: visited
        FROM_ASSET: visited
        name: FROM_ASSET
      name: COMPUTE_PER_FACE
    COMPUTE_PER_VERTEX: visited
    FROM_ASSET:
      COMPUTE_PER_FACE:
        COMPUTE_PER_FACE: visited
        COMPUTE_PER_VERTEX: visited
        FROM_ASSET: visited
        name: COMPUTE_PER_FACE
      COMPUTE_PER_VERTEX: visited
      FROM_ASSET: visited
      name: FROM_ASSET
    name: COMPUTE_PER_VERTEX
  FROM_ASSET:
    COMPUTE_PER_FACE:
      COMPUTE_PER_FACE: visited
      COMPUTE_PER_VERTEX:
        COMPUTE_PER_FACE: visited
        COMPUTE_PER_VERTEX: visited
        FROM_ASSET: visited
        name: COMPUTE_PER_VERTEX
      FROM_ASSET: visited
      name: COMPUTE_PER_FACE
    COMPUTE_PER_VERTEX:
      COMPUTE_PER_FACE:
        COMPUTE_PER_FACE: visited
        COMPUTE_PER_VERTEX: visited
        FROM_ASSET: visited
        name: COMPUTE_PER_FACE
      COMPUTE_PER_VERTEX: visited
      FROM_ASSET: visited
      name: COMPUTE_PER_VERTEX
    FROM_ASSET: visited
    name: FROM_ASSET
  name: FROM_ASSET
# 该部分定义了网格的法线模式，并且基于面或顶点来计算法线。这些设置对于在渲染中表现出正确的光照效果非常重要。此处“visited”可能是调试输出或者是状态标识符。

min_particle_mass: 9.999999960041972e-13 
# 最小粒子质量，值非常小（接近零），适用于极轻的粒子或小质量对象仿真，可能用于流体或气体仿真中。

override_com: false 
# 是否覆盖质心，false表示保持默认质心，不进行自定义调整。

override_inertia: false 
# 是否覆盖惯性，false表示使用默认的惯性张量，仿真时会使用自动计算的物体惯性。

replace_cylinder_with_capsule: true 
# 用胶囊体替换圆柱体。值为true表示当使用圆柱体时，会自动使用更简便的胶囊体进行替换，这有助于物理仿真中的性能优化，特别是在碰撞检测时。

slices_per_cylinder: 20 
# 圆柱体的切片数量，20个切片代表圆柱体的几何精度，较高的切片数会提供更精细的形状，但可能会增加计算开销。

tendon_limit_stiffness: 1.0 
# 肌腱限制的刚度系数，值为1.0表示适中的刚度，能够有效限制关节的伸展，但不会太过僵硬。

thickness: 0.009999999776482582 
# 肌腱的厚度，接近0.01，表示这些物理连接部件具有非常小的厚度，用于模拟精细的关节或连接。

use_mesh_materials: false 
# 是否使用网格的材料属性，false表示不使用网格自带的材质信息，而是可能使用其他定义的材质。

use_physx_armature: true 
# 使用PhysX引擎的Armature（骨架）仿真。true表示启用该物理仿真功能，以增强仿真的真实感和物理表现。

vhacd_enabled: false 
# 是否启用凸包分解。false表示不使用凸包分解，可能意味着使用的是更简单的碰撞体。

vhacd_params: 
  alpha: 0.05 
  # α参数，控制凸分解的平滑度。值为0.05表示平滑效果适中，适合需要精确形状的物体。
  
  beta: 0.05 
  # β参数，控制凸分解的细节。值为0.05同样是适中值，适用于需要精确几何细节的物体。
  
  concavity: 0.0 
  # 凹度参数，0.0表示不允许凹形部分，所有形状都必须为凸形，适合物理优化。
  
  convex_hull_approximation: true 
  # 是否使用凸包近似，true表示会使用这种方法简化形状，提高仿真速度。

  convex_hull_downsampling: 4 
  # 下采样等级，4表示形状的多边形数量会减少到原来的1/4，提高性能。

  max_convex_hulls: 64 
  # 最大凸包数量，64表示最多可以使用64个凸包来分解形状，适合复杂物体。

  max_num_vertices_per_ch: 64 
  # 每个凸包的最大顶点数，64表示每个凸包中最多可以包含64个顶点。

  min_volume_per_ch: 0.0001 
  # 每个凸包的最小体积，0.0001是一个非常小的值，允许生成较小的凸包。

  mode: 0 
  # 模式参数，0通常表示基本模式。

  ocl_acceleration: true 
  # 启用OpenCL加速，true表示将利用OpenCL进行计算加速。

  pca: 0 
  # 主成分分析参数，0表示未启用。

  plane_downsampling: 4 
  # 平面下采样等级，值为4，意味着平面形状将减少到原始复杂度的1/4。

  project_hull_vertices: true 
  # 是否投影凸包顶点，true表示启用投影，以确保顶点落在物体表面。

  resolution: 100000 
  # 分辨率参数，100000表示非常高的几何分辨率，适合精细的物体仿真。

→__init_buffers

前面初始化环境envs什么的都还常规，平地地形，ee的设定也算常规，大致没啥需要特别注意的

奖励函数

1. `actuation_rate`

输入：actuation_history（历史的驱动信号，总大小4096,30，这里取前面的4096,10和中间的4096,10两部分），dt（时间步长），decimation（控制采样间隔）
输出：该函数返回的是驱动信号变化率的惩罚。
计算过程：
- 首先，计算了两个相邻时刻的驱动信号变化，即：
  $actuation_history ( t ) − actuation_history ( t − 1 ) ) 2 ( dt ⋅ decimation ) 2 \text{error} = \frac{{(\text{actuation\_history}(t) - \text{actuation\_history}(t-1))^2}}{{(\text{dt} \cdot \text{decimation})^2}}$
- 最后返回的是所有信号变化的总和的负值：
  $\text{reward} = - \sum \text{error}$

2. `actuation_rate2`

输入：与 actuation_rate 类似，但这里使用了三帧历史信号。也就是用了actuation_history的三段
输出：三帧信号变化率的惩罚。
计算过程：
- 通过以下公式计算信号的二阶差分：
  $actuation_history ( t ) − 2 ⋅ actuation_history ( t − 1 ) + actuation_history ( t − 2 ) ) 2 ( dt ⋅ decimation ) 2 \text{error} = \frac{{(\text{actuation\_history}(t) - 2 \cdot \text{actuation\_history}(t-1) + \text{actuation\_history}(t-2))^2}}{{(\text{dt} \cdot \text{decimation})^2}}$
- 返回总和的负值：
  $\text{reward} = - \sum \text{error}$

3. `ang_vel_xy`

输入：base_ang_vel（基础角速度，4096,3，这里只取x 和 y 方向也就是4096,2）
输出：x 和 y 轴的角速度惩罚。
计算过程：
- 对角速度平方进行惩罚：
  $base_ang_vel x 2 + base_ang_vel y 2 ) \text{reward} = - \sum (\text{base\_ang\_vel}_{x}^2 + \text{base\_ang\_vel}_{y}^2)$

4. `base_heading`

功能：计算目标朝向command_heading与期望基座self.base_heading朝向的误差并返回相应的奖励。
公式：
$command_heading = atan2 ( self.commands [ : , 1 ] , self.commands [ : , 0 ] ) \text{command\_heading} = \text{atan2}(\text{self.commands}[:, 1], \text{self.commands}[:, 0])$
$base_heading_error = ∣ wrap_to_pi ( command_heading − self.base_heading ) ∣ \text{base\_heading\_error} = |\text{wrap\_to\_pi}(\text{command\_heading} - \text{self.base\_heading})|$
$base_heading_error π 2 ⋅ σ \text{reward} = -e^{-\frac{\text{base\_heading\_error}}{\frac{\pi}{2}\cdot \sigma}}$

5. `base_height`

$self.cfg.rewards.base_height_target − self.base_height \text{error} = \text{self.cfg.rewards.base\_height\_target} - \text{self.base\_height}$
$\text{reward} = e^{-\frac{(\text{error}/1)^2}{\sigma}}$

6. `base_z_orientation`

self.projected_gravity 4096,3

功能：计算与理想竖直方向的误差，并返回奖励。
公式：
$self.projected_gravity [ : , : 2 ] ∥ \text{error} = \|\text{self.projected\_gravity}[:, :2]\|$
$\text{reward} = e^{-\frac{(\text{error}/0.2)^2}{\sigma}}$

7. `contact_schedule`

self.step_location_offset 4096,2
self.foot_on_motion 4096,2
self.step_location_offset[~self.foot_on_motion]是按位取反得到4096,1

功能：根据脚的接触情况计算奖励。
公式：
$contact_rewards = ( self.foot_contact [ : , 0 ] − self.foot_contact [ : , 1 ] ) × self.contact_schedule \text{contact\_rewards} = (\text{self.foot\_contact}[:,0] - \text{self.foot\_contact}[:,1]) \times \text{self.contact\_schedule}$
$tracking_rewards = 3 ⋅ e − ( self.step_location_offset[not self.foot_on_motion] ) 2 σ \text{tracking\_rewards} = 3 \cdot e^{-\frac{(\text{self.step\_location\_offset[not self.foot\_on\_motion]})^2}{\sigma}}$
$contact_rewards × tracking_rewards \text{reward} = \text{contact\_rewards} \times \text{tracking\_rewards}$

8. `dof_pos_limits`

输入：dof_pos（关节位置 4096,10），dof_pos_limits（关节位置的上下限分别是10长）
输出：关节位置接近极限的惩罚。
计算过程：
- 计算关节位置超过上下限的偏差：
  $out_of_limits = − ( dof_pos − dof_pos_limits lower ) + ( dof_pos − dof_pos_limits upper ) \text{out\_of\_limits} =- (\text{dof\_pos} - \text{dof\_pos\_limits}_{\text{lower}}) + (\text{dof\_pos} - \text{dof\_pos\_limits}_{\text{upper}})$
- 返回惩罚：
  $out_of_limits \text{reward} = - \sum \text{out\_of\_limits}$

9. `dof_vel`

输入：dof_vel（关节速度 4096,10）
输出：关节速度的惩罚。
计算过程：
- 对所有关节速度平方求和进行惩罚：
  $dof_vel 2 \text{reward} = - \sum \text{dof\_vel}^2$

10. `joint_regularization`

self.dof_pos 4096,10

功能：惩罚关节的位置偏差，鼓励保持对称性。
公式：
$self.dof_pos[:,i] self.scales[’dof_pos’] ) 2 \text{error} = \sum_{i\in {0,1,5,6}} e^{-(\frac{\text{self.dof\_pos[:,i]}}{\text{self.scales['dof\_pos']}})^2}$
$\text{reward} = \frac{\text{error}}{4}$

对应left/right hip yaw/Abduction

11. `lin_vel_z`

输入：base_lin_vel（基础线速度 4096,3，z 方向所以是4096,1）
输出：基础线速度 z 轴方向的惩罚。
计算过程：
- 直接对 z 轴速度的平方进行惩罚：
  $base_lin_vel z ) 2 \text{reward} = - (\text{base\_lin\_vel}_{z})^2$

12. `torque_limits`

输入：torques（力矩 4096,10），torque_limits（力矩上限 10长），soft_torque_limit（软力矩限制系数）
输出：接近力矩极限的惩罚。
计算过程：
- 对力矩超过上限的部分进行惩罚：
  $torque_limits ⋅ soft_torque_limit ) \text{reward} = - \sum \left( |\text{torques}| - \text{torque\_limits} \cdot \text{soft\_torque\_limit} \right)$

13. `torques`

输入：torques（作用在关节上的力矩）
输出：关节力矩的惩罚。
计算过程：
- 对所有关节的力矩平方求和进行惩罚：
  $\text{reward} = - \sum \text{torques}^2$

14. `tracking_lin_vel_world`

功能：计算与命令的线性速度的误差，并返回奖励。
公式：
$self.root_states [ : , 7 : 9 ] 1 + ∣ self.commands [ : , : 2 ] ∣ \text{error} = \frac{\text{self.commands}[:, :2] - \text{self.root\_states}[:, 7:9]}{1+|\text{self.commands}[:, :2]|}$
$\text{reward} = e^{-\frac{\text{error}^2}{\sigma}}$

15. `termination`

输入：reset_buf（重置缓冲区），timed_out（是否超时）
输出：终止条件的惩罚或奖励。
计算过程：
- 如果发生了终止（超时或其他条件），则返回惩罚：
  $reset_buf ⋅ ¬ timed_out ) \text{reward} = - (\text{reset\_buf} \cdot \neg \text{timed\_out})$

train_cfg

algorithm:
  PPO:
    clip_param: 0.2
    # PPO 算法中的裁剪参数。值为 0.2 表示在更新策略时，策略的变化不能超过 20%。这有助于防止更新过度，保持稳定训练。

    desired_kl: 0.01
    # 希望的 KL 散度目标。0.01 表示 PPO 在更新时会监控策略的变化，如果 KL 散度超出此值，学习率会动态调整或停止更新。这是一个较小的值，有助于稳定训练。

    entropy_coef: 0.01
    # 熵的系数，增加探索行为。值为 0.01 表示加入轻微的随机性以防止策略过度收敛到次优解。

    gamma: 0.99
    # 折扣因子，控制奖励的时间衰减。0.99 是一个常见值，表示未来的奖励比当前奖励稍微不重要，但仍然考虑长远收益。

    lam: 0.95
    # GAE (Generalized Advantage Estimation) 的 λ 参数。值为 0.95 表示在计算优势函数时，引入了平滑性，既考虑了当前奖励也考虑了长期收益。

    learning_rate: 1.0e-05
    # 学习率。1e-05 是一个非常小的值，表示模型更新时步伐很小，适合稳定训练。

    max_grad_norm: 1.0
    # 梯度裁剪的最大值。值为 1.0 表示限制梯度的最大范数，防止梯度爆炸，保证训练稳定。

    num_learning_epochs: 5
    # 每个训练周期中的学习次数。5 次是一个较常见的值，适合大部分任务，平衡了计算效率和训练效果。

    num_mini_batches: 4
    # 每个学习周期中的 mini-batch 数量。值为 4 表示将数据分成 4 组来计算梯度更新，适用于并行化训练，能够有效利用计算资源。

    schedule: adaptive
    # 学习率的调整策略。‘adaptive’ 表示根据训练情况自适应地调整学习率。

    use_clipped_value_loss: true
    # 是否使用裁剪的价值函数损失。值为 true 表示启用这一特性，以防止价值函数估计出现剧烈波动。

    value_loss_coef: 1.0
    # 价值函数损失的权重。值为 1.0 表示损失函数的值函数部分对总损失有相同的贡献。

do_wandb: true
# 是否启用 Weights and Biases（W&B）工具进行实验监控和日志记录。值为 true 表示开启。

policy:
  actions:
  - dof_pos_target
  # 动作选择，'dof_pos_target' 代表关节位置目标，表示策略输出控制关节的目标位置。

  activation: elu
  # 激活函数。使用 ELU（Exponential Linear Unit），它能够处理负数输入，具有较好的梯度特性，适合深度网络。

  actor_hidden_dims:
  - 256
  - 256
  - 256
  # Actor 网络的隐藏层维度。三层 256 神经元的结构表示策略网络的复杂性，适合处理较复杂的输入输出关系。

  actor_obs:
  - base_height
  - base_lin_vel_world
  - base_heading
  - base_ang_vel
  - projected_gravity
  - foot_states_right
  - foot_states_left
  - step_commands_right
  - step_commands_left
  - commands
  - phase_sin
  - phase_cos
  - dof_pos
  - dof_vel
  # Actor 网络的输入观测。这些观测包括了机器人状态（如高度、线速度、角速度、重力方向等），关节状态（如位置和速度），以及步态控制命令等，涵盖了机器人控制所需的关键信息。

  critic_hidden_dims:
  - 256
  - 256
  - 256
  # Critic 网络的隐藏层维度，与 Actor 网络相同，也是三层 256 神经元。

  critic_obs:
  - base_height
  - base_lin_vel_world
  - base_heading
  - base_ang_vel
  - projected_gravity
  - foot_states_right
  - foot_states_left
  - step_commands_right
  - step_commands_left
  - commands
  - phase_sin
  - phase_cos
  - dof_pos
  - dof_vel
  # Critic 网络的输入观测，与 Actor 网络的输入相同。这些观测用于评估当前策略下的价值函数。

  init_noise_std: 1.0
  # 初始噪声标准差。值为 1.0 表示在初始策略训练中加入了较大的噪声，有助于增加探索性。

  noise:
    base_ang_vel: 0.05
    base_heading: 0.01
    base_height: 0.05
    base_lin_vel: 0.05
    base_lin_vel_world: 0.05
    commands: 0.1
    dof_pos: 0.05
    dof_vel: 0.5
    foot_contact: 0.1
    foot_states_left: 0.01
    foot_states_right: 0.01
    projected_gravity: 0.05
    step_commands_left: 0.05
    step_commands_right: 0.05
  # 动作和状态的噪声标准差，控制不同状态下的随机扰动幅度。例如：
  # - `base_ang_vel: 0.05` 表示基础角速度加入 0.05 的随机扰动，模拟不确定性；
  # - `dof_vel: 0.5` 表示关节速度有较大的噪声影响，表示对速度变化的不确定性较高。

  normalize_obs: true
  # 是否对观测进行归一化。值为 true 表示对输入的观测值进行归一化处理，有助于网络训练稳定。

runner:
  algorithm_class_name: PPO
  # 使用的算法类为 PPO（Proximal Policy Optimization），这是深度强化学习中常用的策略优化算法。

  experiment_name: Humanoid_Controller
  # 实验名称为 'Humanoid_Controller'，方便记录实验数据。

  max_iterations: 5000
  # 最大迭代次数，值为 5000 表示总共运行 5000 个训练迭代。

  num_steps_per_env: 24
  # 每个环境中的步骤数，值为 24 表示每个环境在更新之前经历 24 步。

  plot_input_gradients: false
  # 是否绘制输入的梯度图像，值为 false 表示不绘制。

  plot_parameter_gradients: false
  # 是否绘制参数的梯度图像，值为 false 表示不绘制。

  policy_class_name: ActorCritic
  # 使用的策略类为 ActorCritic 模型，该模型包含 Actor（策略网络）和 Critic（价值网络）两部分。

  run_name: sf
  # 运行名称，设置为 'sf'，通常用于区分不同的运行配置。

  save_interval: 100
  # 保存间隔。每隔 100 次迭代保存模型，便于检查训练过程中的进展。

seed: -1
# 随机种子。-1 表示使用随机种子，这会导致每次运行的结果有所不同，常用于不同实验的随机性。

OnPolicyRunner.init → BaseTask.reset → LeggedRobot.reset_envs

其实就相当于原本的reset_idx

OnPolicyRunner.init → BaseTask.reset → LeggedRobot.reset_envs → HumanoidController._reset_system
这个函数的目的是在环境复位时重置机器人的状态，包括足部状态、步态命令、步态状态等其他辅助状态。整个函数基于输入的 env_ids 对环境中的多个实例进行操作。

1. 先继承LeggedRobot，这部分其实就相当于原本的_reset_dofs + _reset_root_states

2. 足部状态的计算：self.foot_states[env_ids]
```
self.foot_states[env_ids] = self._calculate_foot_states(self.rigid_body_state[:, self.feet_ids, :7])[env_ids]
```
- 作用：调用 _calculate_foot_states 函数，基于机器人刚体状态（self.rigid_body_state）计算机器人当前的足部状态，并将结果存储在 self.foot_states 中。
- 输入：self.rigid_body_state[:, self.feet_ids, :7] 提取了足部相关的刚体状态，其中包括了足部位置（[:3]）和四元数表示的旋转（[3:7]）。
  
  _calculate_foot_states
  - 作用：该函数通过将足部的高度向量（foot_height_vec）应用于足部的旋转（由四元数表示），计算出足部相对于地面的真实位置。
  - 公式解释：
    - quat_apply 用于将四元数旋转应用到 foot_height_vec 上：
      $rfoot_height_vec_in_world = quat_apply ( foot_states : , 0 , 3 : 7 , foot_height_vec ) \text{rfoot\_height\_vec\_in\_world} = \text{quat\_apply}(\text{foot\_states}_{:,0,3:7}, \text{foot\_height\_vec})$
      $lfoot_height_vec_in_world = quat_apply ( foot_states : , 1 , 3 : 7 , foot_height_vec ) \text{lfoot\_height\_vec\_in\_world} = \text{quat\_apply}(\text{foot\_states}_{:,1,3:7}, \text{foot\_height\_vec})$
    - 通过这些旋转向量，更新足部的实际位置：
      $foot_states : , 0 , : 3 + = rfoot_height_vec_in_world \text{foot\_states}_{:,0,:3} += \text{rfoot\_height\_vec\_in\_world}$
      $foot_states : , 1 , : 3 + = lfoot_height_vec_in_world \text{foot\_states}_{:,1,:3} += \text{lfoot\_height\_vec\_in\_world}$
  - 输出：该函数返回更新后的足部状态。
3. 足部重力方向初始化：self.foot_projected_gravity
```
self.foot_projected_gravity[env_ids,0] = self.gravity_vec[env_ids]
self.foot_projected_gravity[env_ids,1] = self.gravity_vec[env_ids]
```
- 作用：初始化足部的重力方向，设置左右脚的重力向量。重力向量从 self.gravity_vec 中获取，表示当前环境中重力的方向。
4. 步态命令初始化：self.step_commands
```
self.step_commands[env_ids, 0] = self.env_origins[env_ids] + torch.tensor([0., -0.15, 0.], device=self.device)
self.step_commands[env_ids, 1] = self.env_origins[env_ids] + torch.tensor([0., 0.15, 0.], device=self.device)
```
- 作用：初始化左右脚的步态命令。
  - self.step_commands[env_ids, 0]：右脚初始化位置为环境起始点向左移动 0.15 米。
  - self.step_commands[env_ids, 1]：左脚初始化位置为环境起始点向右移动 0.15 米。
- 意义：设置初始步态位置为左右对称，为后续步伐规划提供参考。
5. 步态运动状态初始化：self.foot_on_motion
```
self.foot_on_motion[env_ids, 0] = False
self.foot_on_motion[env_ids, 1] = True
```
- 作用：初始化步态状态，设置右脚不在运动，而左脚为摆动状态。
6. 步态状态初始化：self.current_step 和 self.prev_step_commands
```
self.current_step[env_ids] = torch.clone(self.step_commands[env_ids])
self.prev_step_commands[env_ids] = torch.clone(self.step_commands[env_ids])
```
- 作用：将当前步态命令克隆到 current_step 和 prev_step_commands 中，表示当前步态状态和前一次的步态命令。
7. 步态成功状态初始化：self.semi_succeed_step 和 self.succeed_step
```
self.semi_succeed_step[env_ids] = False
self.succeed_step[env_ids] = False
self.already_succeed_step[env_ids] = False
```
- 作用：初始化步态的成功状态，表示当前步态还没有成功，属于失败状态。
8. 其他变量初始化
- self.had_wrong_contact[env_ids] = False：表示没有发生错误接触。
- self.update_count[env_ids] = 0：更新计数初始化为 0。
- self.update_commands_ids[env_ids] = False：是否更新步态命令的标识初始化为 False。
- self.phase_count[env_ids] = 0：步态相位计数初始化为 0。
- self.update_phase_ids[env_ids] = False：步态相位更新标识初始化为 False。
- self.phase[env_ids] = 0：步态相位初始化为 0。
- self.ICP[env_ids] = 0.：初始捕捉点（ICP）设置为 0。
- self.raibert_heuristic[env_ids] = 0.：Raibert 步态启发式算法值初始化为 0。
- self.w[env_ids] = 0.：步态频率初始化为 0。
- self.dstep_length[env_ids] = self.cfg.commands.dstep_length：步长从配置中获取。
- self.dstep_width[env_ids] = self.cfg.commands.dstep_width：步宽从配置中获取。
OnPolicyRunner.init → BaseTask.reset → LeggedRobot.reset_envs → HumanoidController._resample_commands

1. 继承LeggedRobot:
- 这一部分和原来的差不多
- 1. 线速度命令：
  - self.commands[env_ids, 0]：为 lin_vel_x 生成随机值，表示前后方向上的线速度。
  - self.commands[env_ids, 1]：为 lin_vel_y 生成随机值，表示左右方向上的线速度。
- 1. 角速度命令：
  - self.commands[env_ids, 2]：为 yaw_vel 生成随机值，表示机器人的偏航速度，即旋转速度。
- 1. 小速度修正：
  - 如果生成的线速度命令的大小（在 x 和 y 方向上）小于 0.2，那么将其归零。
  - 如果偏航速度的绝对值小于 0.2，则将偏航速度也归零。这避免了机器人在执行微小速度命令时可能导致的无效动作。
2. self.step_period[env_ids]:
- 这里从 command_ranges 中获取 sample_period（步态周期）的上下限，并为每个环境 ID 随机生成一个步态周期。
- 使用 torch.randint() 随机生成的步态周期决定了每一步的持续时间。较大的步态周期意味着每一步的时间较长。
3. self.full_step_period:
- self.full_step_period = 2 * self.step_period 说明完整的步态周期是单步周期的两倍。这通常适用于两足步态或对称步态，每只脚的摆动和支撑时间相同。
4. self.step_stance[env_ids]:
- 将 self.step_period[env_ids] 克隆给 self.step_stance，这意味着步态的站立时间与步态周期直接相关。
5. self.dstep_width[env_ids]:
- 随机选择目标步幅宽度 (dstep_width)。这个值表示机器人每一步左右脚之间的水平距离，通过 torch_rand_float() 在 command_ranges 中指定的范围内随机生成。较大的步幅宽度可能会增加步伐的稳定性，但也会增加行走的难度。

OnPolicyRunner.init → BaseTask.reset → LeggedRobot.step

~~# OnPolicyRunner.init → BaseTask.reset → LeggedRobot.step → BaseTask.reset_buffers~~
~~# OnPolicyRunner.init → BaseTask.reset → LeggedRobot.step → LeggedRobot.pre_physics_step~~ 这前两个基本上没有啥操作
~~# OnPolicyRunner.init → BaseTask.reset → LeggedRobot.step → BaseTask.render~~和原版一样
循环遍历cfg.control.decimation，也就是比原版少了cfg.env.test的判断，多了cfg.asset.disable_motors的判断
OnPolicyRunner.init → BaseTask.reset → LeggedRobot.step → LeggedRobot.post_physics_step
- OnPolicyRunner.init → BaseTask.reset → LeggedRobot.step → LeggedRobot.post_physics_step → HumanoidController._post_physics_step_callback
  1. 继承LeggedRobot
    这里其实比原版就少了cfg.commands.heading_command的判断，多了cfg.terrain.measure_heights的判断
  2. OnPolicyRunner.init → BaseTask.reset → LeggedRobot.step → LeggedRobot.post_physics_step → HumanoidController._post_physics_step_callback → HumanoidController._update_robot_states
    
    这个函数用于更新机器人状态变量，包括基底高度、方向、髋关节位置、足部状态等。
    
    分析：
    - self.base_height[:] = self.root_states[:, 2:3]：更新机器人基底的高度信息（z 轴），通过 root_states 中的第 3 维提取（假设 z 轴为高度方向）。
    - forward = quat_apply(self.base_quat, self.forward_vec)：调用 quat_apply 函数（用于将四元数旋转应用于向量），计算机器人基于当前姿态的前向向量。
      - quat_apply原版也见到过
        quat_apply 用于将四元数旋转应用到一个向量 b 上。该函数输入为四元数 a 和三维向量 b，返回应用四元数旋转后的向量。
        输入：a 是形状为 (n, 4) 的四元数，b 是形状为 (n, 3) 的向量。
        输出：旋转后的向量，形状与 b 相同。
        公式：
        首先提取四元数的向量部分 xyz = a[:, :3] 和标量部分 w = a[:, 3]。
        然后通过交叉乘法计算中间量：
        $\cdot (\text{xyz} \times b)$
        最终计算结果为：
        $\text{result} = b + w \cdot t + \text{xyz} \times t$
        该公式基于四元数旋转的定义。
    - self.base_heading = torch.atan2(forward[:, 1], forward[:, 0]).unsqueeze(1)：通过前向向量计算机器人的朝向角，使用反正切函数 atan2 计算该角度。
    - self.right_hip_pos 和 self.left_hip_pos：更新左右髋关节位置，从 rigid_body_state 中根据 right_hip_yaw 和 left_hip_yaw 提取位置。
    - self.foot_states = self._calculate_foot_states(self.rigid_body_state[:, self.feet_ids, :7])：
      - 调用了 _calculate_foot_states 函数，使用刚体状态（包括位置和四元数）来计算机器人双足的位置和方向。
        _calculate_foot_states 函数用于计算左右脚的状态，将四元数旋转应用到固定的足部高度向量上。
        - 输入：foot_states 是足部的状态，其中包含了四元数和位置信息。
        - 输出：更新后的 foot_states，其中位置包含了旋转后的足部高度。
        - 步骤：
        1. 定义足部高度向量 foot_height_vec，代表足部距离地面的高度（-0.04 米）。
        2. 使用 quat_apply 对足部状态中的四元数进行旋转，计算 rfoot_height_vec_in_world 和 lfoot_height_vec_in_world，表示左右脚的旋转后的高度。
        3. 更新 foot_states，将旋转后的高度向量加到左右脚的当前位置上。
    - right_foot_forward 和 left_foot_forward：再次使用 quat_apply 计算左右脚的前向向量。
    - right_foot_heading 和 left_foot_heading：计算左右脚的方向（航向角），使用 atan2 函数，然后通过 wrap_to_pi 将角度限制在 [-π, π] 范围内。
      - wrap_to_pi 函数用于将角度限制在 [-π, π] 的范围内，确保角度不会超出正常的范围。
        - 输入：angles 是需要被限制的角度。
        - 输出：限制后的角度，范围在 [-π, π] 之间。
        - 公式：
        1. 使用取余运算将角度限制在 [0, 2π]：
        $\text{angles} \%= 2\pi$
        2. 如果角度超过 π，减去 2π，使其回到 [-π, π] 范围：
        $\text{angles} -= 2\pi \cdot (\text{angles} > \pi)$
    - self.foot_projected_gravity：通过 quat_rotate_inverse 计算重力矢量在脚部坐标系中的投影，更新左右脚的重力向量。
      - quat_rotate_inverse 用于计算一个向量 v 在四元数 q 的逆旋转下的结果。
      - 输入：q 是形状为 (n, 4) 的四元数，v 是形状为 (n, 3) 的向量。
      - 输出：旋转后的向量。
      - 公式：
        将四元数的标量部分提取为 q_w = q[:, -1]，向量部分提取为 q_vec = q[:, :3]。
        计算三个分量：
        - $\cdot (2 \cdot q_w^2 - 1)$
        - $\cdot (q_w \cdot (q_vec \times v))$
        - $\cdot q_vec \cdot (q_vec \cdot v)$
        最终结果为：
        $\text{result} = a - b + c$
    - self.foot_contact = torch.gt(self.contact_forces[:, self.feet_ids, 2], 0)：判断脚是否接触地面，通过检测 z 轴方向上的接触力来判断是否大于 0。
      - torch.gt(intput, other) 判断是否input > other
    - self.contact_schedule = self.smooth_sqr_wave(self.phase)：调用 smooth_sqr_wave 函数，根据相位生成平滑的方波（可能用于控制步态切换），更新接触状态。
      - smooth_sqr_wave 生成一个基于相位的平滑方波，用于控制机器人步态的相位变化。
      - 输入：phase 是步态相位（通常在 [0, 1] 范围内）。
      - 输出：返回平滑的方波值。
      - 公式：
        首先将相位映射到角度：
        $2\pi \cdot \text{phase}$
        然后计算平滑方波：
        $\text{wave} = \frac{\sin(p)}{\sqrt{\sin(p)^2 + \epsilon^2}}$
        其中 $\epsilon = 0.2$ 用于避免奇点，使得波形更加平滑。
    - current_step_masked：更新当前步态的状态，根据当前的接触状态，更新 current_step 以反映当前的脚部位置和航向角。
      - self.current_step[self.foot_contact]：这一步是通过 self.foot_contact 筛选出那些与地面接触的脚，并从 self.current_step 中选取这些脚的当前步态命令。self.foot_contact 是一个布尔掩码，它的维度与环境中的脚数相同，标识哪些脚当前接触地面。
        
        具体的 self.foot_contact：根据代码的其他部分，self.foot_contact 是通过检测 contact_forces 中 z 轴方向的接触力来确定的。因此，self.foot_contact 是一个布尔向量，表示哪些脚与地面有接触。
      - current_step_masked = self.current_step[self.foot_contact]：从 self.current_step 中提取那些正在接触地面的脚的当前步态信息。self.current_step 是一个 3 维的张量，每只脚有 3 个维度信息（通常是 [x, y, heading]，即平面坐标和航向角）。
      - current_step_masked[:, :2] = self.foot_states[self.foot_contact][:,:2]：这一步更新 current_step_masked 中步态命令的前两个维度（即 x 和 y 坐标）。这里通过访问 self.foot_states[self.foot_contact][:, :2]，将对应接触地面的脚的 foot_states 的 x 和 y 坐标值赋给 current_step_masked。
        
        self.foot_states：记录了脚的位置信息和方向。在这里，我们更新了脚步状态的 x 和 y 坐标，以确保它们与当前真实的脚步状态保持一致。
      - current_step_masked[:, 2] = self.foot_heading[self.foot_contact]：这一步将当前接触地面脚的航向角更新到 current_step_masked 中的第 3 维（即 heading 值），这里的航向角来自 self.foot_heading，代表左右脚的旋转方向。
      - self.current_step[self.foot_contact] = current_step_masked：最终，将 current_step_masked 中更新的步态信息写回到 self.current_step 中，确保接触地面的脚的步态命令被更新。
      - 总结：
        current_step_masked 是从 self.current_step 中根据接触地面的脚提取出的一个子集，并在 x、y 坐标和航向角（heading）上进行了更新，最后这些修改后的步态信息被写回 self.current_step 中。
    - self.ankle_vel_history：更新脚踝的速度历史，通过滑动窗口保存之前的速度信息，用于后续的步态分析或控制。
      - naxis = 3：这是一个简单的标志，表示在更新 ankle_vel_history 时，前三个维度是要存储脚踝的速度信息。
      - self.ankle_vel_history[:,0,naxis:] = self.ankle_vel_history[:,0,:naxis]：这一步更新了右脚脚踝的速度历史。它将当前的脚踝速度（前三个维度）保存到历史记录的后一部分。
        
        self.ankle_vel_history 是一个张量，用于存储脚踝的速度历史，第一维度代表环境 ID，第二维度代表左右脚（0 是右脚，1 是左脚），第三维度用于存储速度数据。
        naxis 标志着前三个维度（代表当前的脚踝速度）需要被保存到历史记录的下一部分。
      - self.ankle_vel_history[:,0,:naxis] = self.rigid_body_state[:, self.rigid_body_idx['right_foot'], 7:10]：这一步更新右脚当前的速度信息，速度信息从 rigid_body_state 的第 7 到 10 维提取（这通常表示刚体的线速度分量 x、y、z）。
        
        self.rigid_body_state[:, self.rigid_body_idx['right_foot'], 7:10] 代表右脚的速度向量（x, y, z）。
      - self.ankle_vel_history[:,1,naxis:] = self.ankle_vel_history[:,1,:naxis]：与右脚的更新类似，这一步将当前左脚的速度保存到 ankle_vel_history 的历史部分。
      - self.ankle_vel_history[:,1,:naxis] = self.rigid_body_state[:, self.rigid_body_idx['left_foot'], 7:10]：这一步从 rigid_body_state 中提取左脚的速度信息，并更新到 ankle_vel_history 中的当前部分（前三个维度）。
      - 总结：
      - self.ankle_vel_history 用于保存左右脚脚踝的速度历史信息，代码逻辑通过滚动窗口的方式存储脚踝的最新速度，并保持前一时刻的速度数据。这样做的目的是为了在步态控制或分析中使用过去的脚踝速度信息，例如用于步态预测或控制中的速度平滑。
      - 每次更新时，最新的速度值会被存储到 ankle_vel_history 的前三个维度，而历史速度信息会向后移位。这样可以保持最新和前一段时间的速度历史。
  3. OnPolicyRunner.init → BaseTask.reset → LeggedRobot.step → LeggedRobot.post_physics_step → HumanoidController._post_physics_step_callback → self._calculate_CoM()
    
    这个函数用于计算机器人的质心（Center of Mass，CoM）。
    - 计算公式：
      $rigid_body_state i ⋅ rigid_body_mass i ) mass_total \text{CoM} = \frac{\sum (\text{rigid\_body\_state}_{i} \cdot \text{rigid\_body\_mass}_{i})}{\text{mass\_total}}$
      - rigid_body_state[:,:,:3] 提取每个刚体的质心位置，rigid_body_mass.unsqueeze(1) 代表每个刚体的质量。通过逐个刚体的质心位置乘以其质量，最后除以总质量，得到整个机器人的质心。
  4. OnPolicyRunner.init → BaseTask.reset → LeggedRobot.step → LeggedRobot.post_physics_step → HumanoidController._post_physics_step_callback → self._calculate_raibert_heuristic()
    
    这个函数基于 Raibert 启发式算法，计算下一步的位置和角度。
    - g = -self.sim_params.gravity.z：获取重力加速度。
    - k = torch.sqrt(self.CoM[:,2:3] / g)：计算 Raibert 参数， $k$ 值与重力和质心高度相关，反映步态平衡的调节因子。
    - p_symmetry = 0.5 * self.step_stance * self.dt * self.base_lin_vel_world[:,:2] + k * (self.base_lin_vel_world[:,:2] - self.commands[:,:2])：
      - $p_{\text{symmetry}} = 0.5 \cdot t_{\text{stance}} \cdot v + k \cdot (v - v_{\text{cmd}})$
      - 这个公式表示对称步态的位置调节项，基于机器人当前的线速度和命令速度。
    - 更新右脚和左脚位置：通过 Raibert 启发式算法，计算左右髋关节的下一步位置，反映步态规划的结果。
  5. OnPolicyRunner.init → BaseTask.reset → LeggedRobot.step → LeggedRobot.post_physics_step → HumanoidController._post_physics_step_callback → self._calculate_ICP()
    
    这个函数用于计算机器人的瞬时捕捉点（Instantaneous Capture Point，ICP）。
    - 计算公式：
      $\sqrt{\frac{g}{z}}$
      其中 $g$ 是重力加速度， $z$ 是质心的高度。
      
      ICP 公式为：
      $x_{\text{ic}} = x + \frac{x'}{w}, \quad y_{\text{ic}} = y + \frac{y'}{w}$
      其中 $x$ 和 $y$ 是质心的位置， $x^{'}$ 和 $y^{'}$ 是质心的速度。
  6. OnPolicyRunner.init → BaseTask.reset → LeggedRobot.step → LeggedRobot.post_physics_step → HumanoidController._post_physics_step_callback → self._measure_success_rate()
    
    这个函数用于计算步态命令执行的成功率。
    - step_location_offset：通过计算足部状态与步态命令的差异，测量步态位置偏差：
      $step_location_offset = ∥ foot_states position − step_commands position ∥ \text{step\_location\_offset} = \|\text{foot\_states}_{\text{position}} - \text{step\_commands}_{\text{position}}\|$
    - step_heading_offset：计算步态航向角的偏差，通过 wrap_to_pi 函数将航向角限制在 [-π, π] 范围内：
      $step_heading_offset = ∣ foot_heading − step_commands heading ∣ \text{step\_heading\_offset} = |\text{foot\_heading} - \text{step\_commands}_{\text{heading}}|$
    - 成功步态条件：
      - 位置偏差小于 succeed_step_radius。
      - 航向偏差小于 succeed_step_angle。
    - 错误接触判断：
      - self.had_wrong_contact 表示是否发生了错误接触，逻辑条件是当前和前一次的步态都未成功，并且有脚接触了地面。
    - succeed_step：最终的成功步态条件是当前步态成功并且没有错误接触。
  7. OnPolicyRunner.init → BaseTask.reset → LeggedRobot.step → LeggedRobot.post_physics_step → HumanoidController._post_physics_step_callback → self._update_commands()
    
    这个函数用于更新步态命令。
    - 相位更新：当 phase_count 超过 full_step_period 时重置相位计数，重新开始一个步态周期。
    - 步态命令更新：当 update_count 超过 step_period 时，重置步态命令，并重新生成下一步的步态命令。
    - foot_on_motion 切换：确保至少有一只脚处于运动状态，否则机器人无法更新步态命令。
    - _update_LIPM_CoM 用于更新基于 LIPM 模型的质心（CoM）位置。
      - 输入：update_commands_ids 是需要更新的环境 ID。
      - 输出：更新后的 LIPM 模型质心位置。
      - 步骤：
        提取 right_step_ids 和 left_step_ids，用于标识哪些脚在运动。
        根据支持脚的位置和当前质心位置，计算相对于支持脚的初始位置 x_0 和 y_0。
        使用 LIPM 公式计算最终位置和速度：
        $x_f = x_0 \cdot \cosh(T \cdot w) + v_x \cdot \sinh(T \cdot w) / w$
        $v_{x_f} = x_0 \cdot w \cdot \sinh(T \cdot w) + v_x \cdot \cosh(T \cdot w)$
        更新 LIPM 模型下的质心位置。
    - 生成新步态命令：
      - 调用 HumanoidController._generate_step_command_by_3DLIPM_XCoM，基于 3D LIPM（线性倒立摆模型）和扩展捕捉点（XCoM）生成新的步态命令。
        
        该函数根据 3D LIPM 和 XCoM 模型生成新的步态命令。
        
        输入：update_commands_ids 是需要更新的环境 ID。
        
        输出：生成的步态命令。
        
        步骤：
        
        计算 CoM 的位置和速度相对于支撑脚的偏差。
        使用 LIPM 公式计算下一步的位置和速度。
        结合 XCoM 模型，生成步态命令偏移量，并计算步态命令的最终位置和方向。
    - 脚步碰撞调整：如果左右脚的步态命令之间距离太近（小于 0.2），调用 HumanoidController._adjust_foot_collision 函数调整步态命令。
      - 该函数用于在双脚步态命令发生碰撞时调整脚步的位置。
        
        输入：collision_step_commands 是碰撞的步态命令，collision_foot_on_motion 是发生碰撞的脚部标志。
        输出：调整后的步态命令。
        步骤：
        计算双脚步态命令之间的距离。
        根据碰撞脚的标志，调整脚步位置，使得两个脚之间保持一定的距离，避免碰撞。
    - 地形适应：如果配置中启用了测量高度选项，调用 HumanoidController._adjust_step_command_in_rough_terrain 函数进行地形调整。
      - 该函数用于在粗糙地形上调整步态命令的高度。
      - 输入：update_commands_ids 是需要更新的环境 ID，update_step_commands_mask 是步态命令的遮罩。
      - 输出：调整后的步态命令高度。
      - 步骤：
        
        使用 quat_apply_yaw 将机器人的基底四元数应用于地形高度点，得到世界坐标系下的高度点。
        
        该函数用于将输入的四元数 quat 旋转仅应用于 yaw（绕 z 轴旋转）方向的分量，而忽略其他旋转（pitch 和 roll）。函数的目的是将向量 vec 经过绕 z 轴的旋转后得到新的向量。这在步态控制或基于地面的机器人的方向控制中很常见，因为我们通常只关心机器人在水平面的旋转。
        
        quat_yaw = quat.clone().view(-1, 4)：
        
        首先，函数将输入的四元数 quat 复制一份，并将其形状调整为 (n, 4)，其中 n 是输入的批次大小，每个四元数的维度是 4。四元数通常以 (x, y, z, w) 形式表示，其中 (x, y, z) 是向量部分，w 是标量部分。
        
        quat_yaw[:, :2] = 0.：
        
        这一步将四元数的 x 和 y 分量设置为 0，保持 z 和 w 不变。这意味着我们消除了绕 x 和 y 轴的旋转，只保留了绕 z 轴（yaw 方向）的旋转。这一步确保了我们只对 yaw 方向进行旋转，而忽略了 pitch 和 roll。
        
        quat_yaw = normalize(quat_yaw)：
        
        这一步调用了 isaacgym.torch_utils.normalize 函数，对四元数进行归一化操作。四元数的归一化是为了确保旋转的有效性（保持四元数的单位长度），归一化的四元数确保了旋转矩阵的正交性。
        
        return quat_apply(quat_yaw, vec)：
        
        最后，函数调用 quat_apply，将新的 quat_yaw 四元数应用到向量 vec 上。quat_apply 是之前定义的函数，它将四元数的旋转应用于输入的向量，返回旋转后的结果。
        由于 quat_yaw 只保留了 yaw（绕 z 轴）的分量，因此这一步将仅对 vec 进行 z 轴旋转，而忽略了其他旋转。
        
        总结：
        
        输入：
        
        quat：表示输入的四元数，用于描述机器人的姿态。
        vec：表示需要旋转的向量，通常是空间中的某个点或方向向量。
        
        输出：
        
        旋转后的向量，仅在 yaw（z 轴）方向上进行了旋转。
        
        函数的主要作用：将四元数的旋转应用到向量上，但只保留 z 轴方向的旋转。这对于只关心机器人在水平面（x-y 平面）旋转的场景非常有用，例如步态规划或机器人导航中只关心偏航角的情况。
- OnPolicyRunner.init → BaseTask.reset → LeggedRobot.step → LeggedRobot.post_physics_step → HumanoidController.check_termination
  - 该函数用于检查环境是否需要重置，依据多种条件来决定是否标记为“终止”。
    
    接触力的终止条件
```
term_contact = torch.norm(self.contact_forces[:, self.termination_contact_indices, :], dim=-1)
self.terminated = torch.any((term_contact > 1.), dim=1)
```
    - term_contact：
      - 计算接触力的 L2 范数，具体涉及 contact_forces 的一部分，这些力是机器人与地面或其他物体接触时的反作用力。通过 self.termination_contact_indices 获取感兴趣的接触点。
    - self.terminated：
      - 检查是否有任一接触力的模大于 1。torch.any 函数用于判断在每个环境中是否存在超过这个阈值的接触力。如果超过，则该环境被标记为“终止”。
    速度和高度的终止条件
```
self.terminated |= torch.any(
    torch.norm(self.base_lin_vel, dim=-1, keepdim=True) > 10., dim=1)
```
    - 检查基础线速度（base_lin_vel）的 L2 范数是否大于 10。如果某个环境的线速度超过这个阈值，意味着机器人可能失去控制，因此标记为终止。
```
self.terminated |= torch.any(
    torch.norm(self.base_ang_vel, dim=-1, keepdim=True) > 5., dim=1)
```
    - 检查基础角速度（base_ang_vel）的 L2 范数是否大于 5。如果超过，表示机器人的旋转速度过快，可能导致不稳定，进而需要重置。
```
self.terminated |= torch.any(
    torch.abs(self.projected_gravity[:, 0:1]) > 0.7, dim=1)
self.terminated |= torch.any(
    torch.abs(self.projected_gravity[:, 1:2]) > 0.7, dim=1)
```
    - 检查重力投影在 x 和 y 方向上的分量是否超过 0.7。这通常与机器人的倾斜度或水平稳定性有关，超出该范围可能表示机器人处于不稳定状态。
```
self.terminated |= torch.any(self.base_pos[:, 2:3] < 0.3, dim=1)
```
    - 检查基础位置的 z 轴分量（高度）是否低于 0.3。如果机器人坠落或高度过低，则应重置。
    超时条件
```
self.timed_out = self.episode_length_buf > self.max_episode_length
```
    - 计算是否超出最大允许的回合长度。如果当前回合的长度超过 max_episode_length，则该环境被标记为超时。超时通常不给予终止奖励。
    综合重置标志
```
self.reset_buf = self.terminated | self.timed_out
```
    - 将所有的终止条件（接触力、速度、高度等）以及超时条件结合起来，生成一个综合的重置标志 reset_buf。如果任何一个条件为真，标记为需要重置。
    总结
    - check_termination 函数通过多重条件（包括接触力、速度、倾斜度和高度）来判断环境是否需要重置，并综合处理超时情况。
    - 这个函数确保机器人在接触不稳定、速度过快或坠落时能够及时重置，以维持训练过程的有效性和安全性。
- 更新动作历史记录
```
nact = self.num_actuators
self.actuation_history[:, 2*nact:] = self.actuation_history[:, nact:2*nact]
self.actuation_history[:, nact:2*nact] = self.actuation_history[:, :nact]
self.actuation_history[:, :nact] = self.dof_pos_target * self.cfg.control.actuation_scale
```
  - nact = self.num_actuators：将当前的执行器数量存储到 nact 中，后续的操作将依赖于这个值。
  - self.actuation_history[:, 2*nact:] = self.actuation_history[:, nact:2*nact]：
    - 将当前的执行历史（actuation_history）中的第二段（即前 nact 个执行器的历史）移动到最后一段的位置。这是一个滑动窗口的操作，将历史记录向前推移。
  - self.actuation_history[:, nact:2*nact] = self.actuation_history[:, :nact]：
    - 将历史记录中的第一段（当前执行器的目标位置 dof_pos_target）移动到中间的位置，更新当前的历史记录。
  - self.actuation_history[:, :nact] = self.dof_pos_target * self.cfg.control.actuation_scale：
    - 将目标关节位置（乘以缩放因子）更新到历史记录的第一段。这表示当前的执行器目标位置被记录下来。
- 更新关节速度历史记录
```
self.dof_vel_history[:, nact:] = self.dof_vel_history[:, :nact]
self.dof_vel_history[:, :nact] = self.dof_vel
```
- self.dof_vel_history[:, nact:] = self.dof_vel_history[:, :nact]：
  - 类似于动作历史，更新关节速度的历史记录，将当前的速度记录推移到后半部分。
- self.dof_vel_history[:, :nact] = self.dof_vel：
  - 将当前的关节速度（dof_vel）更新到速度历史记录的前半部分，保持对最新速度的追踪。
OnPolicyRunner.init → BaseTask.reset → LeggedRobot.step → LeggedRobot.post_physics_step → HumanoidController._visualization

1 清除旧的绘制
```
self.gym.clear_lines(self.viewer)
```
在可视化窗口中清除之前绘制的线条，为新一轮的绘制做准备。

2 _draw_world_velocity_arrow_vis
- 绘制机器人的线性和角速度箭头
```
def _draw_world_velocity_arrow_vis(self):
    origins = self.base_pos + quat_apply(self.base_quat, torch.tensor([0.,0.,.5]).repeat(self.num_envs, 1).to(self.device))
    lin_vel_command = torch.cat((self.commands[:, :2], torch.zeros((self.num_envs, 1), device=self.device)), dim=1) / 5
    for i in range(self.num_envs):
        lin_vel_arrow = VelCommandGeometry(origins[i], lin_vel_command[i], color=(0, 1, 0))
        gymutil.draw_lines(lin_vel_arrow, self.gym, self.viewer, self.envs[i], pose=None)
```
- 解析：
  - origins：计算箭头的起点，添加了偏移量以显示在机器人正前方 0.5 米的高度。
  - lin_vel_command：获取当前的线性速度命令并将其缩放，以便在可视化中显示。
  - for i in range(self.num_envs):：为每个环境绘制速度箭头，通过 humanoid_utils.VelCommandGeometry 生成箭头对象，然后调用 gymutil.draw_lines 将箭头绘制到可视化中。
  - VelCommandGeometry 是一个用于表示速度命令的几何图形类，继承自 LineGeometry。它主要用于可视化机器人的线性或角速度命令，通常以箭头的形式展示在可视化界面中。
    
    1. 初始化方法 __init__
```
def __init__(self, origin:torch.Tensor, vel_command:torch.Tensor, color:Tuple):
```
    - 参数：
      - origin: 箭头的起始点，表示速度命令的起点位置。
      - vel_command: 线性或角速度命令，表示箭头的方向和长度。
      - color: 箭头的颜色，以元组的形式提供（例如 RGB 颜色）。
    2. 箭头的顶点计算
```
tip = origin + vel_command 
verts = np.empty((1, 2), gymapi.Vec3.dtype)
```
    - tip：计算箭头的顶点位置，即起始点 origin 加上速度命令 vel_command。这确定了箭头的方向和长度。
    - verts：初始化一个数组，准备存储箭头的两个端点，箭头的起始点和顶点。
    3. 箭头结构定义
```
verts[0][0] = tuple(origin.tolist())
verts[0][1] = tuple(tip.tolist())
```
    - 将箭头的起始点和顶点存储到 verts 数组中。通过调用 tolist() 方法将 torch.Tensor 转换为普通的 Python 列表，然后转化为元组。
    4. 颜色设置
```
colors = np.empty(1, gymapi.Vec3.dtype)
colors.fill(color)
self._colors = colors
```
    - 颜色数组：创建一个用于存储箭头颜色的数组，并将所有的颜色值设置为传入的 color 参数。
    5. 方法 vertices 和 colors
```
def vertices(self):
    return self.verts

def colors(self):
    return self._colors
```
    - vertices 方法：返回箭头的顶点信息，供可视化使用。
    - colors 方法：返回箭头的颜色信息，供可视化使用。
    总结
    - 目的：VelCommandGeometry 类用于创建表示线性或角速度命令的箭头，便于在可视化界面中展示机器人的运动状态。
    - 功能：
      - 计算箭头的起始点和顶点。
      - 设置箭头的颜色。
      - 提供访问箭头顶点和颜色的方法。
    该类可以帮助开发者直观地理解机器人的运动和指令，通过可视化提高对机器人行为的理解和调试效率。
3 _draw_step_vis()
绘制当前脚步位置
```
def _draw_step_vis(self):
    for i in range(self.num_envs):
        right_foot_step = FootStepGeometry(self.current_step[i, 0, :2], self.current_step[i, 0, 2], color=(1, 0, 1))  # Right foot: Pink
        left_foot_step = FootStepGeometry(self.current_step[i, 1, :2], self.current_step[i, 1, 2], color=(0, 1, 1))  # Left foot: Cyan
        gymutil.draw_lines(left_foot_step, self.gym, self.viewer, self.envs[i], pose=None)
        gymutil.draw_lines(right_foot_step, self.gym, self.viewer, self.envs[i], pose=None)
```
- 解析：
  - for i in range(self.num_envs):：为每个环境绘制左右脚步的几何图形，分别用不同颜色表示（粉色和青色）。
  - FootStepGeometry：构造表示步态的几何对象。
    - 用于表示机器人的脚步几何图形，具体描述了脚步的位置、方向和颜色等属性。它可能用于可视化机器人的步态规划或实际步态的展示。
      
      1. 初始化方法 __init__
```
def __init__(self, step_location:torch.Tensor, step_orientation:torch.Tensor, color:Tuple):
```
      - 参数：
        step_location: 表示脚步的位置（x, y）。
        step_orientation: 表示脚步的方向（航向角，以弧度表示）。
        color: 表示脚步的颜色，以元组形式传入（例如 RGB 颜色）。
      2. 设备和位置处理
```
self.device = step_location.device
step_location = torch.tensor([*step_location, 0.], device=self.device)  # (x, y, 0)
```
      - 设备设置：将脚步位置的设备信息（CPU/GPU）保存到类实例中。
      - 位置转换：将脚步位置扩展为三维向量 (x, y, 0)，适配后续的计算。
      3. 角度和点数设置
```
angle = torch.tensor([60])  # angle between each point on circle's circumference
num_lines = int(360 / angle.item()) + 1
verts = np.empty((num_lines, 2), gymapi.Vec3.dtype)
```
      - 角度定义：定义了每个点之间的角度为 60 度，这将用于生成圆形脚步的边界点。
      - 点数计算：通过 360 度除以每个点之间的角度，计算出需要的点的数量。
      4. 单位旋转矩阵函数
```
unit_rotation_matrix_func = lambda angle: torch.tensor([[torch.cos(torch.tensor([angle])), -torch.sin(torch.tensor([angle])), 0],
                                                        [torch.sin(torch.tensor([angle])), torch.cos(torch.tensor([angle])),  0],
                                                        [0,                                0,                                 1]],
                                                        device=self.device)
```
      - 函数定义：定义一个函数，用于生成给定角度的二维旋转矩阵。该矩阵用于在计算脚步位置时进行旋转变换。
      5. 脚步结构生成
      
      以下部分用于生成脚步的几何结构：
```
offset = torch.tensor([0.1, 0., 0.], dtype=torch.float, device=self.device)
offset = torch.matmul(unit_rotation_matrix_func(step_orientation), offset.T)
verts[0][0] = tuple((step_location + offset).tolist())
```
      - 脚步偏移：定义一个初始的偏移量，表示脚步相对于脚步位置的偏移。
      - 旋转计算：使用之前定义的旋转矩阵函数，根据 step_orientation（航向角）对偏移量进行旋转，得到脚步的真实位置。
      - 脚步点生成：循环执行以上逻辑，生成多个脚步的几何点，以形成脚步的边界。
      6. 脚步颜色设置
```
colors = np.empty(num_lines, gymapi.Vec3.dtype)
colors.fill(color)
self._colors = colors
```
      - 颜色数组：生成一个颜色数组，为每个脚步边界点填充指定的颜色。
      7. 方法 vertices 和 colors
```
def vertices(self):
    return self.verts

def colors(self):
    return self._colors
```
      - vertices 方法：返回脚步几何的顶点信息。
      - colors 方法：返回脚步几何的颜色信息。
      总结
      
      FootStepGeometry 类定义了一个用于可视化机器人的脚步几何结构的对象，主要包括脚步的位置、方向和颜色等信息。它通过旋转矩阵生成脚步的轮廓，并提供了方法以便访问这些顶点和颜色数据。这使得在可视化过程中能够准确地展示机器人的脚步状态。
  - gymutil.draw_lines：将脚步的几何形状绘制到可视化中。
_draw_step_command_vis
绘制当前步态命令
```
def _draw_step_command_vis(self):
    for i in range(self.num_envs):
        right_step_command = FootStepGeometry(self.step_commands[i, 0, :2], self.step_commands[i, 0, 2], color=(1, 0, 0))  # Right foot: Red
        left_step_command = FootStepGeometry(self.step_commands[i, 1, :2], self.step_commands[i, 1, 2], color=(0, 0, 1))  # Left foot: Blue
        gymutil.draw_lines(left_step_command, self.gym, self.viewer, self.envs[i], pose=None)
        gymutil.draw_lines(right_step_command, self.gym, self.viewer, self.envs[i], pose=None)
```
- 与 _draw_step_vis 类似，此处绘制的是当前步态命令（左脚和右脚），分别用红色和蓝色表示。

wandb

在runner.learn之前，主函数多了些wandb的东西

1. WandbSingleton 的创建

wandb_helper = wandb_singleton.WandbSingleton()

wandb_helper：创建一个 WandbSingleton 的实例，通常用于与 Weights & Biases（W&B）服务交互。W&B 是一个用于机器学习实验跟踪和可视化的工具。

def __new__(self):
    if not hasattr(self, 'instance'):
        self.instance = super(WandbSingleton, self).__new__(self)
        self.entity_name = None
        self.project_name = None
        self.experiment_name = ''
        self.enabled = False
        self.parameters_dict = None
    return self.instance

功能：重写 __new__ 方法以实现单例模式。如果尚未创建 instance 属性，则创建一个新的实例并初始化一些属性（如 entity_name、project_name 等）。
使用场景：在创建 wandb_helper 实例时，确保只有一个实例在程序运行期间被使用，避免多次初始化导致的错误。

2. W&B 的设置

wandb_helper.setup_wandb(env_cfg=env_cfg, train_cfg=train_cfg, args=args, log_dir=policy_runner.log_dir)

setup_wandb 方法：配置 W&B 环境，通常包括以下内容：
- env_cfg：传入环境配置，可能包含关于环境的参数和设置。
- train_cfg：传入训练配置，包含训练过程中的超参数和设置。
- args：其他参数或命令行参数，提供运行时的额外信息。
- log_dir：日志目录，指定存储 W&B 日志和模型检查点的位置。
这个步骤通常会初始化 W&B 项目，设置实验名称、超参数、日志目录等，以便于后续的实验跟踪和结果可视化。

def set_wandb_values(self, train_cfg, args):
    wandb_config_path = os.path.join(LEGGED_GYM_ROOT_DIR, 'user', 'wandb_config.json')

    if os.path.exists(wandb_config_path):
        json_data = json.load(open(wandb_config_path))
        print('Loaded WandB entity and project from JSON.')
        self.entity_name = json_data['entity']
        self.project_name = train_cfg.runner.experiment_name
    if args.wandb_entity is not None:
        print('Received WandB entity from arguments.')
        self.entity_name = args.wandb_entity
    if args.wandb_project is not None:
        print('Received WandB project from arguments.')
        self.project_name = args.wandb_project
    if args.task is not None:
        self.experiment_name = f'{args.task}'

    if (self.entity_name is None or self.project_name is None
       or args.disable_wandb):
        self.enabled = False
    else:
        print(f'Setting WandB project name: {self.project_name}\n' +
              f'Setting WandB entity name: {self.entity_name}\n')
        self.enabled = True

功能：从 JSON 配置文件加载 W&B 的 entity 和 project 名称。如果在命令行参数中提供了这些值，则覆盖配置文件中的值。
使用场景：当调用 wandb_helper.setup_wandb(...) 时，会首先调用这个方法来初始化 W&B 的基本设置。

3. 记录和保存代码

local_code_save_helper.log_and_save(
    env, env_cfg, train_cfg, policy_runner)

local_code_save_helper.log_and_save 方法：用于记录和保存当前的代码和配置。此方法可能执行以下操作：
- env：当前环境实例，用于记录实验所用的环境。
- env_cfg：环境配置，保存当前环境的设置。
- train_cfg：训练配置，保存当前训练的超参数和设置。
- policy_runner：策略运行器，可能包含模型和训练过程中的状态信息。
此步骤有助于创建实验的完整记录，确保可重复性，尤其是在机器学习实验中，记录代码和配置是非常重要的。

4. 将策略运行器与 W&B 关联

wandb_helper.attach_runner(policy_runner=policy_runner)

attach_runner 方法：将策略运行器（policy_runner）与 W&B 关联，以便跟踪与该运行器相关的所有指标和信息。
这通常意味着将 policy_runner 中的训练过程、奖励、损失等数据上传到 W&B 进行可视化和分析。

def attach_runner(self, policy_runner):
    if not self.is_wandb_enabled():
        return
    policy_runner.attach_to_wandb(wandb)

功能：将策略运行器（policy_runner）与 W&B 关联，使其能够记录训练过程中的指标。
使用场景：在调用 wandb_helper.attach_runner(...) 时，确保 W&B 启用后，将策略运行器附加到 W&B。

总结

这段代码的作用是：

创建并配置一个 W&B 实例，以便进行实验跟踪和可视化。
记录当前实验的代码和配置，以确保可重复性。
将策略运行器与 W&B 关联，以便跟踪其训练过程和相关数据。

OnPolicyRunner.learn

1. 初始化和设置

if self.logging_cfg['enable_local_saving']:
    self.logger.make_log_dir()
if init_at_random_ep_len:
    self.env.episode_length_buf = torch.randint_like(self.env.episode_length_buf, high=int(self.env.max_episode_length))

创建日志目录：如果启用了本地保存配置，调用 self.logger.make_log_dir() 来创建日志保存的目录。
随机化回合长度：如果 init_at_random_ep_len 为 True，随机设置当前环境的回合长度，确保多样性。
1. 提高训练的多样性
  多样性：随机化回合长度会导致每个环境在不同时间长度下运行，这增加了训练过程中情境的多样性。不同的回合长度意味着模型将接触到各种状态和奖励结构，有助于提高泛化能力。
2. 避免过拟合
  避免训练过度拟合：如果所有环境的回合长度都相同，模型可能会过拟合于这种特定的情况。通过引入随机性，模型更难以记住固定模式，这有助于避免过拟合，并使模型在面对新情况时更加灵活。
3. 探索不同策略
  探索与利用：在强化学习中，代理需要平衡探索新策略与利用已知策略的收益。随机化回合长度可能会导致代理尝试不同的策略，帮助其找到更优的解决方案。
4. 处理稀疏奖励
  稀疏奖励环境：在某些环境中，代理可能在每个回合中很少获得奖励。通过随机化回合长度，代理可以在不同的情境中探索，增加获得奖励的机会，最终提升学习效率。

2. 获取观测值

actor_obs = self.get_noisy_obs(self.policy_cfg["actor_obs"])
critic_obs = self.get_obs(self.policy_cfg["critic_obs"])

获取演员观测值：调用 get_noisy_obs 方法，传入配置中的观测参数（也就是前面cfg中的['base_height', 'base_lin_vel_world', 'base_heading', 'base_ang_vel', 'projected_gravity', 'foot_states_right', 'foot_states_left', 'step_commands_right', 'step_commands_left', 'commands', 'phase_sin', 'phase_cos', 'dof_pos', 'dof_vel']这些），获取带噪声的观测值。
OnPolicyRunner.learn → OnPolicyRunner.get_noisy_obs → OnPolicyRunner.get_obs → HumanoidController._set_obs_variables
- 此方法用于设置和更新与观察相关的变量，特别是与机器人的足部状态、步态命令和基座速度等有关的状态变量。
  1. 计算右脚和左脚的状态
```
self.foot_states_right[:,:3] = quat_rotate_inverse(self.base_quat, self.foot_states[:,0,:3] - self.base_pos)
self.foot_states_left[:,:3] = quat_rotate_inverse(self.base_quat, self.foot_states[:,1,:3] - self.base_pos)
```
  - 右脚状态：
    - 计算右脚的位置，使用 quat_rotate_inverse 方法将脚的位置转换到世界坐标系下。
    - self.foot_states[:,0,:3] 是右脚的当前状态，self.base_pos 是基座的当前位置。
  - 左脚状态：
    - 同样的计算逻辑用于左脚，更新 self.foot_states_left。
  1. 计算脚的朝向
```
self.foot_states_right[:,3] = wrap_to_pi(self.foot_heading[:,0] - self.base_heading.squeeze(1))
self.foot_states_left[:,3] = wrap_to_pi(self.foot_heading[:,1] - self.base_heading.squeeze(1))
```
  - 脚的朝向：
    - 使用 wrap_to_pi 方法确保脚的朝向相对于基座朝向的偏差保持在 [-π, π] 的范围内。
  1. 计算步态命令的状态
```
self.step_commands_right[:,:3] = quat_rotate_inverse(self.base_quat, torch.cat((self.step_commands[:,0,:2], torch.zeros((self.num_envs,1), device=self.device)), dim=1) - self.base_pos)
self.step_commands_left[:,:3] = quat_rotate_inverse(self.base_quat, torch.cat((self.step_commands[:,1,:2], torch.zeros((self.num_envs,1), device=self.device)), dim=1) - self.base_pos)
```
  - 步态命令的位置：
    - 计算右脚和左脚的步态命令，并将其位置转换到世界坐标系。步态命令的 x 和 y 坐标通过与基座位置的差值计算得到。
```
self.step_commands_right[:,3] = wrap_to_pi(self.step_commands[:,0,2] - self.base_heading.squeeze(1))
self.step_commands_left[:,3] = wrap_to_pi(self.step_commands[:,1,2] - self.base_heading.squeeze(1))
```
  - 步态命令的朝向：类似于脚的朝向，更新步态命令的方向。
  1. 计算相位的正弦和余弦值
```
self.phase_sin = torch.sin(2*torch.pi*self.phase)
self.phase_cos = torch.cos(2*torch.pi*self.phase)
```
  - 相位计算：根据当前的相位值计算正弦和余弦，这通常用于运动学中的周期性运动表示。
  1. 获取世界线性速度
```
self.base_lin_vel_world = self.root_states[:, 7:10].clone()
```
  - 基础线性速度：从根状态中提取线性速度，通常用于更新控制输入或评估当前状态。
OnPolicyRunner.learn → OnPolicyRunner.get_noisy_obs → OnPolicyRunner.get_obs → HumanoidController.get_states
- base_height: 表示机器人的基座高度，形状为 torch.Size([4096, 1])，每个环境实例有一个高度值。
- base_lin_vel_world: 表示机器人的世界坐标系下的线性速度，形状为 torch.Size([4096, 3])，包含 x、y 和 z 方向的速度分量。
- base_heading: 表示机器人的基座朝向，形状为 torch.Size([4096, 1])，用于指示机器人的方向。
- base_ang_vel: 表示机器人的世界坐标系下的角速度，形状为 torch.Size([4096, 3])，包含关于 x、y 和 z 轴的角速度分量。
- projected_gravity: 表示在机器人坐标系下的重力投影，形状为 torch.Size([4096, 3])，包含重力在各个方向上的分量。
- foot_states_right: 表示右脚的状态，形状为 torch.Size([4096, 4])，包含位置和朝向信息。
- foot_states_left: 表示左脚的状态，形状为 torch.Size([4096, 4])，同样包含位置和朝向信息。
- step_commands_right: 表示右脚的步态命令，形状为 torch.Size([4096, 4])，包含目标位置和朝向。
- step_commands_left: 表示左脚的步态命令，形状为 torch.Size([4096, 4])，同样包含目标位置和朝向。
- commands: 表示当前的动作命令，形状为 torch.Size([4096, 3])，通常包括线性和角速度。
- phase_sin: 表示相位的正弦值，形状为 torch.Size([4096, 1])，用于周期性运动的表示。
- phase_cos: 表示相位的余弦值，形状为 torch.Size([4096, 1])，与相位的正弦值一起用于周期性运动的表示。
- dof_pos: 表示机器人的关节位置，形状为 torch.Size([4096, 10])，每个环境实例有 10 个关节的位置。
- dof_vel: 表示机器人的关节速度，形状为 torch.Size([4096, 10])，每个环境实例有 10 个关节的速度。
通过HumanoidController.get_state获取上述这14个观察值后拼接得到4096,52
加入噪声扰动 $o b s + (2 * r an d o m - 1) * n o i se$ 得到actor_obs
获取评论家观测值：只调用 get_obs 方法，得到无噪声的critic_obs

3. 初始化迭代变量

ep_infos = []
rewbuffer = deque(maxlen=100)
lenbuffer = deque(maxlen=100)
cur_reward_sum = torch.zeros(self.env.num_envs, dtype=torch.float, device=self.device)
cur_episode_length = torch.zeros(self.env.num_envs, dtype=torch.float, device=self.device)

self.num_learning_iterations = num_learning_iterations
tot_iter = self.current_learning_iteration + num_learning_iterations

变量说明：
- ep_infos：存储每个回合的信息。
- rewbuffer 和 lenbuffer：用于保存最近的奖励和回合长度，以便计算平均值。
- cur_reward_sum 和 cur_episode_length：用于跟踪当前回合的总奖励和长度。

4. 主学习循环

for it in range(self.current_learning_iteration, tot_iter):	# 5000
    start = time.time()
    # * Rollout
    with torch.inference_mode():
        for i in range(self.num_steps_per_env):	# 24
            actions = self.alg.act(actor_obs, critic_obs)
            
            self.set_actions(actions)
            # 对动作进行裁剪，确保其在合理范围内，然后将动作传递给环境
            self.env.step()
            # 还是前面的LeggedRobot.step

            dones = self.get_dones()
            # 得到self.env.reset_buf.to(self.device) 4096长，bool元素
            timed_out = self.get_timed_out()
            # 得到self.env.get_state('timed_out').to(self.device)
            # 也是4096长，bool元素
            rewards = self.compute_and_get_rewards()
            self.reset_envs()
            #还是前面的LeggedRobot.reset_envs
            infos = self.get_infos()
            # 得到self.env.extras这个字典，里面其实就还是15项奖励
            
            actor_obs = self.get_noisy_obs(self.policy_cfg["actor_obs"])
            critic_obs = self.get_obs(self.policy_cfg["critic_obs"])

            self.alg.process_env_step(rewards, dones, timed_out)
            # 和原版比就少个self.actor_critic.reset(dones)

########################## 下面这一大块和原版都是一模一样
            # * Book keeping
            ep_infos.append(infos['episode'])
            cur_reward_sum += rewards
            cur_episode_length += 1
            new_ids = (dones > 0).nonzero(as_tuple=False)
            rewbuffer.extend(cur_reward_sum[new_ids][:, 0].cpu().numpy().tolist())
            lenbuffer.extend(cur_episode_length[new_ids][:, 0].cpu().numpy().tolist())
            cur_reward_sum[new_ids] = 0
            cur_episode_length[new_ids] = 0

        top = time.time()
        collection_time = stop - start

        # * Learning step
        start = stop
        self.alg.compute_returns(critic_obs)
        # 和原版一模一样，里面还有个循环self.num_transitions_per_env=24

    mean_value_loss, mean_surrogate_loss = self.alg.update()
    # 和原本一模一样
    stop = time.time()
    learn_time = stop - start
########################## 下面是新的部分
    self.tot_timesteps += self.num_steps_per_env * self.env.num_envs
    self.iteration_time = collection_time + learn_time
    self.tot_time += self.iteration_time

    self.log_wandb(locals())
########################## 从这开始其实和原本又是一模一样的            
    if (it % self.save_interval == 0) and self.logging_cfg['enable_local_saving']:
        self.save(os.path.join(self.log_dir, 'model_{}.pt'.format(it)))
    ep_infos.clear()
    self.current_learning_iteration += 1

if self.logging_cfg['enable_local_saving']:
    self.save(os.path.join(self.log_dir, 'model_{}.pt'.format(self.current_learning_iteration)))

OnPolicyRunner.learn → PPO.act → ActorCritic.act → Actor.act → Actor.update_distribution → RunningMeanStd.forward
对输入4096,52的obs算mean和var 52长
- OnPolicyRunner.learn → PPO.act → ActorCritic.act → Actor.act → Actor.update_distribution → RunningMeanStd.forward → RunningMeanStd._update_mean_var_count_from_moments
  - 功能：根据新的批次的统计量（均值和方差）更新当前的均值、方差和计数。这是用于计算运行均值和方差的常用方法。
  - 参数：
    - mean: 当前的均值。← self.running_mean
    - var: 当前的方差。← self.running_var
    - count: 当前的样本数量。← self.count=4097
    - batch_mean: 新批次的均值。← mean
    - batch_var: 新批次的方差。← var
    - batch_count: 新批次的样本数量。← input.size()[0]=4096
  - 步骤：
    1. 计算均值的变化：
      - 计算新批次均值与当前均值之间的差值：
        $batch_mean − mean \delta = \text{batch\_mean} - \text{mean}$
    2. 计算总样本数量：
      - 计算当前样本总数：
        $tot_count = count + batch_count \text{tot\_count} = \text{count} + \text{batch\_count}$
    3. 更新均值：
      - 通过加权更新当前均值：
        $new_mean = mean + δ ⋅ batch_count tot_count \text{new\_mean} = \text{mean} + \frac{\delta \cdot \text{batch\_count}}{\text{tot\_count}}$
    4. 更新方差：
      - 计算当前方差和新批次方差的加权总和：
        $m_a = \text{var} \cdot \text{count}$
        $batch_var ⋅ batch_count m_b = \text{batch\_var} \cdot \text{batch\_count}$
        $batch_count tot_count M2 = m_a + m_b + \frac{\delta^2 \cdot \text{count} \cdot \text{batch\_count}}{\text{tot\_count}}$
      - 更新方差：
        $new_var = M 2 tot_count \text{new\_var} = \frac{M2}{\text{tot\_count}}$
    5. 更新计数：
      - 新的计数即为总样本数量：
        $new_count = tot_count \text{new\_count} = \text{tot\_count}$
返回更新后的均值、方差和计数，也就是更新self.running_mean, self.running_var, self.count，再赋值给current_mean ，current_var
从而计算 $current_mean current_var + e p s y=\frac{obs-\text{current\_mean}}{\sqrt{\text{current\_var}+eps}}$

从而标准化后的obs输入给mean_NN输出mean 4096,10
从而self.distribution = Normal(mean, mean*0. + self.std)
从而action=self.distribution.sample()

ActorCritic(
  (actor): Actor(
    (obs_rms): RunningMeanStd()
    
    (mean_NN): Sequential(
      (0): Linear(in_features=52, out_features=256, bias=True)
      (1): ELU(alpha=1.0)
      (2): Linear(in_features=256, out_features=256, bias=True)
      (3): ELU(alpha=1.0)
      (4): Linear(in_features=256, out_features=256, bias=True)
      (5): ELU(alpha=1.0)
      (6): Linear(in_features=256, out_features=10, bias=True)
    )
  )
  (critic): Critic(
    (NN): Sequential(
      (0): Linear(in_features=52, out_features=256, bias=True)
      (1): ELU(alpha=1.0)
      (2): Linear(in_features=256, out_features=256, bias=True)
      (3): ELU(alpha=1.0)
      (4): Linear(in_features=256, out_features=256, bias=True)
      (5): ELU(alpha=1.0)
      (6): Linear(in_features=256, out_features=1, bias=True)
    )
    (obs_rms): RunningMeanStd()
  )
)

OnPolicyRunner.learn → OnPolicyRunner.set_actions → BaseTask.set_states
```
def set_states(self, state_list, values):
    idx = 0
    for state in state_list:
        state_dim = getattr(self, state).shape[1]
        self.set_state(state, values[:, idx:idx + state_dim])
        idx += state_dim
    assert(idx == values.shape[1]), "Actions don't equal tensor shapes"
```
- 目的：此方法用于将给定的状态值分配到多个状态变量中。它接受一个状态名称的列表和一个包含相应值的张量。
- 参数：
  - state_list: 包含要设置的状态名称的列表。
  - values: 一个张量，包含各状态的值，形状应与状态名称列表的总维度相匹配。
- 工作流程：
  - 初始化 idx 为 0，用于跟踪在 values 张量中的当前索引位置。
  - 遍历 state_list，对每个状态执行以下操作：
    - 使用 getattr(self, state).shape[1] 获取当前状态变量的维度。
    - 从 values 张量中提取对应的值片段，调用 set_state 方法将其设置为当前状态。
    - OnPolicyRunner.learn → OnPolicyRunner.set_actions → BaseTask.set_states → BaseTask.set_state
```
def set_state(self, name, value):
    try:
        if name in self.scales.keys():
            setattr(self, name, value / self.scales[name])
        else:
            setattr(self, name, value)
    except AttributeError:
        print("Value for " + name + " does not match tensor shape")
```
      - 目的：此方法用于设置单个状态的值，允许根据需要应用缩放。
      - 参数：
        
        name: 要设置的状态名称。
        value: 要赋值的张量。
      - 工作流程：
        
        使用 try 和 except 处理潜在的属性错误。
        检查 name 是否在 self.scales 中：
        如果存在，将 value 除以对应的缩放因子，并将结果赋值给状态属性。
        否则，直接将 value 赋值给状态属性。
        
        如果尝试设置的状态不存在，捕获 AttributeError 并打印错误消息，说明值的形状与状态的形状不匹配。
    - 更新 idx，指向下一个状态在 values 中的起始位置。
  - 最后，使用 assert 确保处理的值的总维度与 values 的第二维度一致。
这里就单纯一个 'dof_pos_target'，也就是policy得到的action作为这个东西