读《Visual Whole-Body for Loco-Manipulation》①low

这个新项目比那些强化学习入门项目好配置多了。。。
不过需要搞个wandb账号，一开始运行时需要api key

args

Namespace(
  checkpoint=-1,
  cols=None,
  compute_device_id=0,
  debug=False,
  experiment_name=None,
  exptid='SOME_YOUR_DESCRIPTION_debug',
  flat_terrain=False,
  flex=False,
  graphics_device_id=0,
  headless=False,
  horovod=False,
  load_run='',
  max_iterations=None,
  num_envs=None,
  num_threads=0,
  observe_gait_commands=False,
  physics_engine=SimType.SIM_PHYSX,
  physx=False,
  pipeline='cuda:2',
  pitch_control=False,
  proj_name='b1z1-low',
  record_video=False,
  resume=False,
  resumeid=None,
  rl_device='cuda:2',
  rows=None,
  run_name=None,
  seed=None,
  sim_device='cuda:2',
  sim_device_id=0,
  sim_device_type='cuda:2',
  slices=0,
  stand_by=False,
  stochastic=False,
  stop_update_goal=False,
  subscenes=0,
  task='b1z1',
  test=False,
  use_gpu=True,
  use_gpu_pipeline=True,
  use_jit=False,
  vel_obs=False
)

其中sim_device pipeline 是args其他配置过程中又设定成了cuda:0，所以最好写死成想要的卡

env_cfg

B1Z1RoughCfg 类继承自 LeggedRobotCfg 类，主要用于定义特定于 B1Z1 机器人在复杂地形下的配置。下面详细分析这个类中的各个部分和参数：
类结构

    B1Z1RoughCfg 作为 LeggedRobotCfg 的扩展，继承了基础配置并对某些特定设置进行了自定义，以适应复杂和多变的环境。

关键属性与方法

    goal_ee：定义了关于机器人末端执行器（end-effector）的目标和轨迹设定。
        num_commands：末端执行器需要遵循的命令数量。
        traj_time 和 hold_time：分别控制轨迹持续时间和保持时间的范围。
        collision_upper_limits 和 collision_lower_limits：定义碰撞检测的上下界限。
        underground_limit：定义了地下限制，用于碰撞检测。
        num_collision_check_samples：在碰撞检测中使用的样本数量。
        command_mode：命令模式，如 'sphere'，指定命令的空间形状。

    noise：定义了加入到系统中的噪声级别和种类，用于模拟现实世界中的不确定性。
        add_noise：是否添加噪声。
        noise_level：噪声的基础级别。
        noise_scales：不同动态变量的噪声强度，如关节位置（dof_pos）和速度（dof_vel）。

    commands：定义了控制命令的重采样时间和课程设置，以及各个命令的范围和计划。
        curriculum：是否使用课程化学习。
        num_commands：命令数量。
        resampling_time：命令重采样的时间。

    env：设置了仿真环境的基本参数。
        num_envs：环境数量。
        num_actions：动作数量。
        episode_length_s：每个仿真环境的时长。

    init_state：定义了初始状态，包括位置、姿态、速度等。
        pos 和 rot：分别定义了初始位置和旋转。
        default_joint_angles：定义了各个关节的默认角度。

    control：控制机器人的动作执行。
        stiffness 和 damping：分别定义了关节的刚度和阻尼。
        action_scale：动作比例因子，决定了实际动作和目标动作的比例关系。

    asset：关于机器人模型和资源的配置。
        file：URDF 文件路径，定义了机器人的物理模型。
        foot_name：脚部名称，用于接触力的检测。

    domain_rand：领域随机化设置，用于提高模型的泛化能力。
        randomize_friction：是否随机化摩擦系数。
        friction_range：摩擦系数的范围。

env_cfg 是 B1Z1RoughCfg 类的实例，那么它的属性将直接对应于该类中定义的成员变量和可能包含的方法。这里是对这些属性的具体说明：

    arm：存储与机械臂相关的配置信息。
    asset：关联于环境中使用的资产或模型的配置。
    box：定义某种类型的盒子或容器的配置。
    commands：存储控制命令或行动的相关配置。
    control：与控制策略或方法相关的配置。
    domain_rand：定义域随机化设置，常用于增强模型的泛化能力。
    env：包含环境的基本配置，如环境尺寸、类型等。
    goal_ee：指的是目标末端执行器的位置或状态。
    init_member_classes：一个方法，用于初始化相关的成员类。
    init_state：定义环境的初始状态。
    noise：配置环境中的噪声参数，如感知噪声或动作噪声。
    normalization：涉及到数据或状态的标准化设置。
    rewards：定义了奖励函数的相关配置。
    seed：设置环境的随机种子，影响随机过程的可复现性。
    sim：与仿真相关的配置，如物理引擎设置等。
    termination：定义了终止条件的相关配置。
    terrain：用于定义地形或地图的配置。
    viewer：与环境的视图或渲染设置相关。

0.5		'bounce_threshold_velocity': 定义了物体接触时的反弹阈值速度。当接触表面的相对速度低于此值时，不会产生反弹。
2		'contact_collection': 指定了接触信息收集的模式。数值为 2 可能代表特定的接触处理策略或优化级别。
0.01	'contact_offset': 定义了接触偏移量，用于调整接触检测算法的灵敏度，较小的值可提高接触检测的准确性，但可能增加计算负担。
5		'default_buffer_size_multiplier': 是一个缓冲区大小的乘数因子，用于动态调整内存分配，以适应不同的模拟需求。
1.0		'max_depenetration_velocity': 指定了最大的穿透速度，用于在物体穿透时限制它们分离的速度，以避免非物理行为。
8388608	'max_gpu_contact_pairs': 定义了GPU上允许的最大接触对数，这是优化GPU计算资源利用的参数。
4		'num_position_iterations': 指定了位置求解器的迭代次数，用于增强模拟的稳定性和准确性。
10		'num_threads': 设置了用于物理计算的线程数，多线程可以提高模拟的处理速度和效率。
0		'num_velocity_iterations': 设置了速度求解器的迭代次数，这影响了动力学响应的准确性和计算开销。
0.0		'rest_offset': 定义了休息偏移量，用于物体在静止时的接触处理，可以帮助防止物体之间的微小抖动。
1		'solver_type': 整数，指定了求解器的类型，数值 1 可能表示使用特定的求解算法或策略。

注册形成环境

这里有需要形成一个task_class变量，他是ManipLoco 类实例，用来形成env变量

ManipLoco

ManipLoco 类继承自 LeggedRobot，是用于操控机械四足机器人的类。该类负责机器人在仿真环境中的运动控制、传感器数据处理、与环境的交互等功能。下面将详细分析这个类的主要方法和属性：

初始化 (init):

ManipLoco 初始化时，首先检查配置 (cfg) 是否包含观察步态命令的选项，如果是，则相应地增加观察向量的长度。
self.stand_by 用来确定是否在待机状态下。
调用基类 LeggedRobot 的初始化方法。

step:

这是类的主要方法之一，用于在每个时间步更新环境状态。
首先处理动作数据，包括限制动作的范围，并可能延迟动作的应用。
调用 render 方法渲染当前帧（如果需要的话）。
更新物理环境，应用力矩等。
最后，刷新状态和计算观测结果，返回新的观测结果和奖励等。

它的主要功能是应用动作，进行仿真，并调用 self.post_physics_step() 来更新环境状态。这段代码包含了一系列复杂的操作，如动作处理、物理仿真、机械臂的逆运动学（IK）控制等。以下是详细的分析：

• 输入参数：

  • actions：一个形状为 (num_envs, num_actions_per_env) 的张量，其中每一行代表一个环境中机器人的动作。

• 功能：

  • 将给定的动作应用到仿真环境中，并进行物理仿真。
  • 通过逆运动学计算机械臂的目标位置，并施加关节扭矩。
  • 更新仿真状态，并返回观测值、奖励、重置状态等。

1. 动作预处理：

   • actions[:, 12:] = 0.：将所有动作中从第12个动作开始的值设为0。
   • actions = self._reindex_all(actions)：
   • • 重新排列动作顺序：这个函数重新排列了 actions 张量的前12个元素，使其按照特定的顺序排列。这可能是因为不同的动作在底层控制中对应不同的关节或功能单元，重新排列能够确保这些动作被正确应用。
   • • 保持其余动作不变：函数保留了从第12个元素开始的动作不变，并将这些动作与重新排列的部分拼接在一起，形成最终的动作张量。
   • actions = torch.clip(actions, -self.clip_actions, self.clip_actions).to(self.device)：将动作限制在 [-self.clip_actions, self.clip_actions] 的范围内，并移动到指定设备（如 GPU）上。

2. 渲染：

   • self.render()：调用渲染函数。如果使用图形界面渲染，则会显示当前仿真环境的状态。
     在 ManipLoco 类的 step 函数中，render 函数的调用用于处理图形渲染和与用户界面的交互。它的作用包括渲染当前仿真环境的视觉输出、处理用户的输入事件（如键盘和鼠标操作），以及同步图形帧的时间。

• 1. 窗口关闭检查:
  • self.gym.query_viewer_has_closed(self.viewer) 用于检查渲染窗口是否被关闭。如果窗口被关闭，程序将通过 sys.exit() 退出运行。
• 2. 相机视角处理:
  • 如果 self.free_cam 为 False，函数会调用 self.lookat(self.lookat_id)，根据特定对象（由 lookat_id 指定）调整摄像机的视角。
• 3. 处理用户输入事件:
  • self.gym.query_viewer_action_events(self.viewer) 获取用户在渲染窗口中的操作（如按键事件），并调用 self.handle_viewer_action_event(evt) 来处理这些事件。
• 4. 同步仿真结果:
  • 如果当前设备不是 cpu，则调用 self.gym.fetch_results(self.sim, True) 来同步仿真结果。
• 5. 更新图形渲染:
  • 如果 self.enable_viewer_sync 为 True，则会调用 self.gym.step_graphics(self.sim) 和 self.gym.draw_viewer(self.viewer, self.sim, True) 来更新图形输出，并通过 self.gym.sync_frame_time(self.sim) 来同步帧时间。
  • 否则，仅调用 self.gym.poll_viewer_events(self.viewer) 来处理用户事件，而不进行图形更新。
• 6. 更新摄像机位置:
  • 如果 self.free_cam 为 False，函数会获取摄像机的当前位置，并将其与目标位置（lookat_id 指定的对象位置）进行比较，计算出摄像机与目标之间的向量 lookat_vec。

在 step 函数中，render 函数的调用用于实现以下功能：

• 1. 视觉输出渲染:
  • 通过调用 render，可以确保仿真环境的图形界面被正确渲染，并呈现给用户。
• 2. 用户输入的处理:
  • render 函数检查并处理用户在渲染窗口中的交互，例如摄像机视角的调整、仿真暂停或退出等操作。这对于仿真环境的实时调整和监控非常重要。
• 3. 仿真帧同步:
  • render 函数确保仿真的图形输出与物理仿真保持同步，使得仿真帧率和图形帧率保持一致，从而提供流畅的视觉体验。

在 step 函数中，render 函数用于渲染仿真环境的视觉输出，处理用户输入事件，并确保仿真帧与图形帧的同步。这使得仿真过程不仅仅是一个计算过程，还能通过图形界面实时展示和交互，从而增强用户的控制和监视能力。

3. 动作延迟处理：
   • 如果 self.action_delay != -1，则在动作历史缓存中添加当前动作，并对动作进行延迟处理。实际应用的动作可能是历史动作而不是当前时刻的动作。

在 ManipLoco 类的 step 函数中，self.action_history_buf 和 self.action_delay 的设置起到了延迟执行动作的作用。

self.action_history_buf 是一个形状为 (6144, 5, 18) 的零张量，其中：

• 6144 表示有 6144 个环境实例。
• 5 表示每个环境实例保存了最近 5 次的动作记录。
• 18 表示每个动作向量有 18 个维度。

self.action_delay 的值为 3，这表示动作将被延迟 3 帧执行。也就是说，当前计算出的动作不会立即在仿真中执行，而是存储在动作历史缓冲区中，并且延迟 3 帧后才会被取出执行。

• • 1. 动作存储:
       在每次调用 step 时，当前计算的动作会追加到 self.action_history_buf 的末尾：

  self.action_history_buf = torch.cat([self.action_history_buf[:, 1:], actions[:, None, :]], dim=1)
  

  这里的 torch.cat 将 self.action_history_buf 的第一个时间维度去掉，并将当前的动作追加到最后。

通过设置 self.action_history_buf 和 self.action_delay，仿真系统能够模拟动作延迟的现实场景。例如，在真实的机器人控制中，由于传感器读取、信号传输和执行器响应的延迟，控制信号通常不会立即被执行。这种延迟模拟可以帮助训练更鲁棒的控制策略，使其在现实世界中更具适应性。

在 step 函数中，self.action_delay 和 self.action_history_buf 确保了仿真动作的执行与实际计算之间存在合理的延迟，从而增强了仿真系统的真实性。

4. 动作缓存：

   • 在仿真步骤小于10000 * 24次时，使用最新的动作（self.action_history_buf[:, -1]）；否则使用较早的动作（self.action_history_buf[:, -2]）。这一机制可能是为了模拟控制延迟或增加稳定性。

5. 机械臂逆运动学控制：

   • 计算机械臂的目标位置：
     • dpos 是目标末端执行器位置与当前末端执行器位置的差值。
     • drot 是目标末端执行器方向与当前末端执行器方向之间的旋转误差。
     • dpose 将位置和旋转误差组合为一个张量，并用于计算机械臂的目标关节位置（arm_pos_targets）。6144,6,1
   • 生成关节目标位置：

     • 使用 self._control_ik(dpose) 计算逆运动学控制器的输出 6144,6，这应该是表示关节速度，指向目标位置。然后将这些目标添加到当前手臂关节的实际位置上，以得到新的位置目标arm_pos_targets 6144,6。（元素加法也算“添加到”吗，数不知道是啥了）

     • all_pos_targets: 初始化一个与 self.dof_pos 大小相同的全零张量，并将 arm_pos_targets 分配到与手臂关节对应的位置。6144,19

6. 物理仿真：

• 在一个控制周期内，根据指定的分辨率（self.cfg.control.decimation = 4），逐步应用计算的扭矩（self._compute_torques(self.actions)），并进行物理仿真。

• 控制器解算:

  • self.torques = self._compute_torques(self.actions): 计算要施加在机器人关节上的力矩（或扭矩）。6144,19

  • self.gym.set_dof_position_target_tensor(self.sim, gymtorch.unwrap_tensor(all_pos_targets)): 将计算得到的关节位置目标传递给仿真引擎，以作为当前的目标位置。

  • self.gym.set_dof_actuation_force_tensor(self.sim, gymtorch.unwrap_tensor(self.torques)): 将计算的力矩应用到仿真中的各个关节上。

• 仿真执行:

  • self.gym.simulate(self.sim): 进行一次物理仿真步，更新仿真状态。

  • • self.gym.fetch_results(self.sim, True): 如果是在CPU设备上时，这里就获取仿真结果。

  • self.gym.refresh_dof_state_tensor(self.sim) 等函数用于刷新关节状态、雅可比矩阵和刚体状态等信息，以确保仿真和实际控制之间的数据同步。

7. 后处理：
   • 仿真结束后，调用 self.post_physics_step() 进行后处理。这通常包括计算奖励、检测终止条件等。
   • 对观测值进行裁剪，以限制其在指定范围内（clip_obs）。
   • 更新全局步骤计数器（self.global_steps），并返回仿真结果，包括观测值、奖励、重置状态等。

• 后处理:

  • self.post_physics_step() 是在仿真步骤完成后的处理函数，可能用于更新状态或进行奖励计算等。

• 裁剪观察结果:

  • torch.clip 函数用于限制观察到的状态在一定范围内，防止状态值过大或过小。

• 返回结果:

  • 返回裁剪后的观测值 6144,744 、奖励、终止标志和其他信息，供进一步的学习算法使用。

• 输入 actions：

  • 形状为 (6144, 18)，表示有 6144 个环境，每个环境有 18 个动作。这些动作在函数开始时被处理并裁剪，最终用于控制机器人。

这个 step 函数将输入的动作应用到仿真环境中，通过一系列处理，包括动作延迟、逆运动学计算、物理仿真等，最终输出环境的新状态和相关信息。代码中涉及到的多次裁剪和缓存处理，表明仿真环境可能有复杂的动作控制和稳定性要求。

post_physics_step:

在物理仿真步骤之后调用，用于计算观测、奖励和是否需要重置环境。
处理碰撞、计算机器人的朝向等。

post_physics_step 是一个关键的函数，用于在物理模拟步骤完成后更新环境的状态、计算观测值和奖励，以及处理环境的重置。这个函数实现了以下功能：

1. 刷新物理状态张量：

   • self.gym.refresh_dof_state_tensor(self.sim) 等多个函数调用用于刷新各类物理状态的张量。这些张量存储了环境中所有实体的状态信息，例如关节状态、根状态、接触力、力传感器状态、刚体状态、雅可比矩阵等。刷新这些张量是为了确保后续的计算基于最新的物理模拟数据。

2. 更新时间步长：

   • self.episode_length_buf += 1 和 self.common_step_counter += 1 用于更新当前episode的步数和全局步数计数器。这有助于跟踪当前episode和整个训练过程中已经执行的步数。

3. 准备物理量：

   • 计算基本的物理量，例如 base_quat 6144,4（根四元数）、base_lin_vel 6144,3（根线速度）、base_ang_vel 6144,3（根角速度）、base_yaw_euler 6144,3（基于欧拉角的根偏航）、base_yaw_quat（基于四元数的根偏航）、projected_gravity（投影重力）等。这些物理量将用于后续的控制和决策过程。
   • contact 6144,4 和 foot_contacts_from_sensor 6144,4 用于检测机器人脚部的接触情况，并通过滤波器 self.contact_filt 进行更新。这些信息在步态控制中非常重要。

4. 调用回调函数：

   • self._post_physics_step_callback() 用于执行一些在物理步骤之后的通用计算。这通常是一个可重载的函数，允许在特定环境中实现自定义的逻辑。

5. 更新末端执行器目标：

   • self._update_curr_ee_goal() 用于更新机器人手臂或其他末端执行器的目标位置。这对实现复杂的操控任务至关重要。

6. 计算观测值、奖励和终止条件：

   • self.check_termination() 用于检查当前环境是否需要终止。
   • self.compute_reward() 用于根据当前状态计算奖励。
   • 如果某些环境需要重置（即 self.reset_buf 中存在非零值），则通过 self.reset_idx(env_ids, start=False) 来重置这些环境。
   • self.compute_observations() 用于计算新的观测值，有时需要在模拟步骤后刷新某些观测值，例如身体位置。

7. 更新历史状态：

   • self.last_actions、self.last_dof_vel、self.last_root_vel、self.last_torques 用于保存上一步的动作、关节速度、根部速度和力矩，以便在下一步计算时使用。

8. 绘制调试可视化：

   • 如果启用了查看器同步（self.enable_viewer_sync）或正在录制视频，则通过 self._draw_ee_goal_curr()、self._draw_ee_goal_traj() 和 self._draw_collision_bbox() 绘制调试信息，例如末端执行器目标、目标轨迹和碰撞包围盒。

_draw_collision_bbox

• 功能:
  这个函数用于在仿真环境中绘制机器人末端执行器（EE）的碰撞边界框（bounding box）。

• 代码解释:

  1. center 变量代表 EE 的目标中心位置。
  2. bbox0 和 bbox1 分别代表碰撞边界框的上限和下限。
  3. 使用 gymutil.WireframeBBoxGeometry 创建边界框的几何体。
  4. 对于每个仿真环境实例，计算边界框的姿态并使用 gymutil.draw_lines 函数绘制。

• 作用:
  通过在仿真环境中绘制碰撞边界框，开发人员可以直观地看到机器人末端执行器在运动过程中是否会触碰到这些边界，帮助调整路径规划或动作控制算法。

_draw_ee_goal_curr

• 功能:
  这个函数用于在仿真环境中绘制机器人末端执行器的当前目标位置，以及当前的末端执行器位置和姿态。

• 代码解释:

  1. sphere_geom 和 sphere_geom_2/3 用于定义不同颜色的球体几何体，分别表示当前目标位置、末端执行器位置和上臂的目标位置。
  2. 使用 gymapi.Transform 创建这些几何体的位姿，并通过 gymutil.draw_lines 绘制在仿真环境中。
  3. axes_geom 用于绘制当前末端执行器姿态的坐标轴。

• 作用:
  这个可视化功能帮助开发人员看到末端执行器的实际位置和预期的目标位置之间的差距，以及它们在空间中的姿态，从而可以更直观地调整控制策略。

_draw_ee_goal_traj

• 功能:
  该函数用于在仿真环境中绘制末端执行器的目标轨迹。

• 代码解释:

  1. 首先生成一系列沿着时间插值的目标点（ee_target_all_sphere），然后将它们从球坐标系转换到笛卡尔坐标系（ee_target_all_cart_world）。
  2. 最终通过 gymutil.draw_lines 在仿真环境中绘制这些轨迹点，显示机器人在执行任务时应该沿着的路径。

• 作用:
  这个功能使得开发人员可以看到机器人应该遵循的理想路径，这对于路径规划算法的调试和验证非常重要。

_resample_commands ：

这个函数用于为指定环境 (env_ids) 重新采样控制命令。
如果 command_env_ids 为 0，则没有环境需要重新采样命令，所以这个函数会直接返回，而不会执行命令重新采样的逻辑。
如果 env_ids 不是 0，那么 command_env_ids 会包含需要重新采样命令的环境的 ID 列表。在这种情况下，以下是函数的行为：

1. 非遥控模式检查：
   首先检查是否处于遥控模式 (teleop_mode)。如果是遥控模式并且 global_steps 小于 5000 * 24，系统将从一个范围内随机采样线速度 lin_vel_x。否则，系统将从另一个范围内随机采样 lin_vel_x。

2. 命令更新：

   • 对于指定的环境（即 env_ids），commands 的第一个元素（线速度 lin_vel_x）会被重新采样并更新。
   • commands 的第二个元素（lin_vel_y）被设置为 0。
   • 第三个元素（ang_vel_yaw，即角速度）也会根据预定的范围重新采样。
   • 最后，较小的命令将被置为 0，通过与指定的阈值 (lin_vel_x_clip 和 ang_vel_yaw_clip) 进行比较来实现。

_step_contact_targets

这个函数的主要任务是更新机器人脚步的接触目标。它根据当前的步态命令 (gait commands)，计算每个脚在特定时间步的接触和摆动状态。具体过程如下：

1. 命令处理：
   如果环境配置中设置了观察步态命令 (observe_gait_commands)，那么会根据步态命令来更新 gait_indices，这些索引用于确定机器人每条腿在当前时间步的接触状态。

2. 步态索引更新：
   步态索引通过当前步态时间 (gait_indices) 和配置的频率 (frequencies) 进行更新，并确保只在行走命令有效时更新这些索引。

3. 脚步接触状态计算：

   • 使用步态索引 (foot_indices) 计算每条腿在当前时间步的接触状态。
   • 通过 smoothing_multiplier 来平滑接触状态，并更新期望的接触状态 (desired_contact_states)。

_post_physics_step_callback

_post_physics_step_callback 是一个在物理仿真步骤之后、计算终止条件、奖励和观测值之前被调用的回调函数。该函数在整个仿真过程中起着重要的作用，主要完成以下几项工作：

1. 命令重新采样：

   • command_env_ids 是在当前时间步需要重新采样命令的环境的索引。通过判断 self.episode_length_buf 是否满足某个条件来确定哪些环境需要重新采样命令。这里的条件是 self.cfg.commands.resampling_time / self.dt，表示命令重新采样的时间间隔。
   • 调用 self._resample_commands(command_env_ids) 函数，为 command_env_ids 中的环境重新生成新的控制命令。这一操作确保机器人在不同时间步内能够接收到新的控制命令，从而进行不同的动作。

2. 步态接触目标更新：

   • self._step_contact_targets() 函数用于更新步态接触目标。这个过程可能涉及根据当前环境状态来调整机器人接触地面的目标，以实现稳定的步态控制。具体的步态控制逻辑和目标更新可能在这个函数内部实现。

3. 机器人随机推力：

   • 如果启用了领域随机化（self.cfg.domain_rand.push_robots），并且当前步数满足推力间隔条件（self.common_step_counter % self.push_interval == 0），那么就会调用 self._push_robots() 函数，对机器人施加随机的推力。
   • 这种随机推力通常用于增强机器人的鲁棒性，让其能够在各种不确定性下保持平衡和稳定。

_post_physics_step_callback 函数主要负责在物理步骤之后对机器人命令进行重新采样、更新步态目标，并在某些情况下对机器人施加随机推力。它为后续的终止条件、奖励和观测值的计算做了准备，并帮助提升机器人的适应性和鲁棒性。

_update_curr_ee_goal 函数分析

这个函数 _update_curr_ee_goal 主要用于更新当前的末端执行器（End-Effector, EE）目标位置和姿态，并在必要时重新采样新目标。以下是对这个函数的详细分析：

1. 非远程操控模式下的目标更新

   • 如果 teleop_mode（远程操控模式）关闭，该部分代码会执行：

     t = torch.clip(self.goal_timer / self.traj_timesteps, 0, 1)
     self.curr_ee_goal_sphere[:] = torch.lerp(self.ee_start_sphere, self.ee_goal_sphere, t[:, None])
     

     • t 是一个时间比例，表示当前时间步进度在整个轨迹时间步数中的比例，范围在 [0, 1] 之间。
     • self.curr_ee_goal_sphere 6144,3 会线性插值（线性插值的系数由 t 确定）更新为当前的末端执行器目标，起点是 self.ee_start_sphere，终点是 self.ee_goal_sphere。

2. 更新末端执行器的笛卡尔坐标系下的目标

   • 通过调用 sphere2cart 函数，将球坐标系下的目标位置转换为笛卡尔坐标系，并存储在 self.curr_ee_goal_cart 6144,3中：

     self.curr_ee_goal_cart[:] = sphere2cart(self.curr_ee_goal_sphere)
     

   • 使用四元数 self.base_yaw_quat 6144,4 对当前的 EE 目标进行旋转变换，并更新为全局坐标系下的目标位置 self.curr_ee_goal_cart_world：

     ee_goal_cart_yaw_global = quat_apply(self.base_yaw_quat, self.curr_ee_goal_cart)
     self.curr_ee_goal_cart_world = self._get_ee_goal_spherical_center() + ee_goal_cart_yaw_global
     

3. 更新末端执行器的目标姿态

   • 使用末端执行器的球坐标计算目标姿态的默认 yaw 和 pitch，并结合 ee_goal_orn_delta_rpy 计算最终的目标姿态四元数：

     default_yaw = torch.atan2(ee_goal_cart_yaw_global[:, 1], ee_goal_cart_yaw_global[:, 0])
     default_pitch = -self.curr_ee_goal_sphere[:, 1] + self.cfg.goal_ee.arm_induced_pitch
     self.ee_goal_orn_quat = quat_from_euler_xyz(self.ee_goal_orn_delta_rpy[:, 0] + np.pi / 2, default_pitch + self.ee_goal_orn_delta_rpy[:, 1], self.ee_goal_orn_delta_rpy[:, 2] + default_yaw)
     

4. 更新计时器并重新采样目标

   • 增加 goal_timer 并检查是否超过了总的轨迹时间步数。如果超过了，则进行目标重新采样：

     self.goal_timer += 1
     resample_id = (self.goal_timer > self.traj_total_timesteps).nonzero(as_tuple=False).flatten()
     

   • 如果 resample_id 不为空并且 stop_update_goal 为 True，则将对应环境的命令置为 0，停止目标更新：

     if len(resample_id) > 0 and self.stop_update_goal:
         self.commands[resample_id, 0] = 0
         self.commands[resample_id, 2] = 0
     

   • 最后，调用 _resample_ee_goal 对需要重新采样的环境 ID 进行新的目标采样。

这个函数在操控任务中很关键，它确保末端执行器的目标位置和姿态随着时间的推移进行更新，并在需要时重新生成新的目标，从而实现更加灵活和智能的操控控制。

计算奖励 (compute_reward):

根据机器人的表现计算奖励，可能包括多种不同的奖励成分，如位置奖励、能量消耗奖励等。

compute_reward 函数的主要作用是计算当前环境中的奖励值，并将这些奖励值累加到相应的缓冲区中。以下是对该函数的详细分析：

1. 初始化奖励缓冲区 (rew_buf):

   self.rew_buf[:] = 0.
   

   • 该部分代码将整个奖励缓冲区 rew_buf 初始化为0，为接下来计算每个奖励函数做准备。

2. 遍历并计算奖励函数:

   for i in range(len(self.reward_functions)):
       name = self.reward_names[i]
       rew, metric = self.reward_functions[i]()
       rew = rew * self.reward_scales[name]
       self.rew_buf += rew
       self.episode_sums[name] += rew
       self.episode_metric_sums[name] += metric
   

   • 这里遍历所有的奖励函数 self.reward_functions。对于每个奖励函数，首先计算得到 rew 和 metric。然后，rew 乘以相应的奖励尺度 self.reward_scales[name] 后，累加到 rew_buf 中。同时，也将 rew 和 metric 累加到 episode_sums 和 episode_metric_sums 中。
     这里没有全部计算ManipLoco_rewards类里的所有函数，而是24个
     
     

3. 奖励值的正向裁剪:

   if self.cfg.rewards.only_positive_rewards:
       self.rew_buf[:] = torch.clip(self.rew_buf[:], min=0.)
   

   • 如果配置中设置了 only_positive_rewards 为 True，则会对 rew_buf 进行裁剪，确保其值不为负。

4. 添加终止奖励:

   if "termination" in self.reward_scales:
       rew, metric = self._reward_termination()
       rew = rew * self.reward_scales["termination"]
       self.rew_buf += rew
       self.episode_sums["termination"] += rew
       self.episode_metric_sums["termination"] += metric
   

   • 如果配置中包含终止奖励（termination），则计算并加入该终止奖励。

5. 奖励值归一化:

   self.rew_buf /= 100
   

   • 最后，将 rew_buf 的值除以100，进行简单的归一化处理。

6. 机械臂奖励的计算:

   • 机械臂奖励部分与上述主机器人奖励部分的流程类似，针对机械臂的奖励函数进行计算和累加。
   • ManipLoco_rewards._reward_tracking_ee_world

compute_reward 函数主要通过遍历多个奖励函数来累加计算奖励，同时处理了奖励的正向裁剪和终止奖励的特殊处理。整个流程确保了在仿真过程中，对不同奖励的适当处理和归一化，以便对环境中的机器人和机械臂进行准确的奖励反馈。

计算观测 (compute_observations):

生成环境的观测值，这可能包括机器人的姿态、速度、关节状态等。
观测值可能会被用于训练机器学习模型。

1. arm_base_pos 的计算:

   arm_base_pos = self.base_pos + quat_apply(self.base_yaw_quat, self.arm_base_offset)
   

   这一行代码通过将机器人底座的旋转四元数（self.base_yaw_quat）应用于一个臂基的偏移量（self.arm_base_offset），来计算臂基相对于底座的位置。然后将这个位置与底座的绝对位置（self.base_pos）相加，得到臂基的世界坐标系位置。

2. ee_goal_local_cart 的计算:

   ee_goal_local_cart = quat_rotate_inverse(self.base_quat, self.curr_ee_goal_cart_world - arm_base_pos)
   

   这一步计算的是当前末端执行器（EE）目标点相对于臂基坐标系的位置。通过将世界坐标系下的 EE 目标点减去臂基位置，并应用底座的逆旋转四元数（self.base_quat），将其转换到臂基的局部坐标系下。

3. 设置命令为零（stand_by 模式）:

   if self.stand_by:
       self.commands[:] = 0.
   

   如果机器人处于待机模式（self.stand_by 为 True），则将所有的命令设置为零，这意味着机器人不会接收任何移动或动作命令。

4. 构建观测张量（obs_buf）: 6144,66

   obs_buf = torch.cat((       self._get_body_orientation(),  # dim 2
                               self.base_ang_vel * self.obs_scales.ang_vel,  # dim 3
                               self._reindex_all((self.dof_pos - self.default_dof_pos) * self.obs_scales.dof_pos)[:, :-self.cfg.env.num_gripper_joints],  # dim 18
                               self._reindex_all(self.dof_vel * self.obs_scales.dof_vel)[:, :-self.cfg.env.num_gripper_joints],  # dim 18
                               self._reindex_all(self.action_history_buf[:, -1])[:, :12],  # dim 12
                               self._reindex_feet(self.foot_contacts_from_sensor),  # dim 4
                               self.commands[:, :3] * self.commands_scale,  # dim 3
                               ee_goal_local_cart,  # dim 3 position
                               0*self.curr_ee_goal_sphere  # dim 3 orientation
                               ),dim=-1)
   

   这部分代码将多种观测值拼接在一起，形成一个完整的观测张量（obs_buf）。各个部分包括：

   • 机器人身体的姿态（2维）
   • 机器人底座的角速度（3维）
   • 关节位置相对于默认位置的偏差（18维，不包括末端执行器的关节）
   • 关节速度（18维，不包括末端执行器的关节）
   • 最近一次的动作历史（12维，只包括腿部关节）
   • 足部接触状态（4维）
   • 线速度命令（3维）
   • 末端执行器目标位置的局部坐标（3维）
   • 末端执行器目标的方向（3维，但被设置为零）

5. 添加步态命令观察:

   if self.cfg.env.observe_gait_commands:
       obs_buf = torch.cat((obs_buf,
                                 self.gait_indices.unsqueeze(1), self.clock_inputs), dim=-1)
   

   如果启用了步态命令观察（observe_gait_commands），则将步态索引和时钟输入添加到观测张量中。

6. 添加私密观察值:

   if self.cfg.domain_rand.observe_priv:
       priv_buf = torch.cat((
           self.mass_params_tensor,
           self.friction_coeffs_tensor,
           self.motor_strength[:, :12] - 1,
       ), dim=-1)
       # 6144,18
       self.obs_buf = torch.cat([obs_buf, priv_buf, self.obs_history_buf.view(self.num_envs, -1)], dim=-1)
       # 6144,744
   

   如果启用了私密观察（observe_priv），则将质量参数、摩擦系数、马达强度等私密信息添加到观测张量中。

7. 更新历史观测数据:

   self.obs_history_buf = torch.where(
       (self.episode_length_buf <= 1)[:, None, None], 
       torch.stack([obs_buf] * self.cfg.env.history_len, dim=1),
       torch.cat([
           self.obs_history_buf[:, 1:],
           obs_buf.unsqueeze(1)
       ], dim=1)
   )
   

   根据当前的观测值更新历史观测缓冲区（obs_history_buf），确保模型在决策时能够访问到过去的观测信息。

8. 添加噪声:

   if self.add_noise:
       self.obs_buf += (2 * torch.rand_like(self.obs_buf) - 1) * self.noise_scale_vec
   

   如果启用了噪声添加（add_noise），则在观测值上叠加随机噪声，以模拟现实中的不确定性。

compute_observations 函数将机器人的传感器数据、状态信息、动作历史以及其他环境特性组合成一个多维的观测张量。这些观测值将用于指导强化学习算法，使机器人能够在复杂的环境中做出合理的决策。通过处理这些观测值，模型能够捕捉到机器人的当前状态及其与目标状态之间的差异，从而更好地控制机器人执行任务。

检查终止条件 (check_termination):

检查是否需要重置环境，例如机器人翻倒或达到最大步数等。

check_termination 函数的作用是检查当前环境是否需要重置，即判断当前仿真环境中的机器人是否达到了终止条件。以下是对该函数的详细分析：

1. 接触力终止条件 (termination_contact_buf):

   termination_contact_buf = torch.any(torch.norm(self.contact_forces[:, self.termination_contact_indices, :], dim=-1) > 1., dim=1)
   

   • 该部分代码检查机器人在指定的接触点（由 self.termination_contact_indices 指定）上的接触力是否超过阈值 1。如果某个环境的任一接触点力大于 1，则认为该环境需要终止并重置。

2. 姿态终止条件 (r_term 和 p_term):

   r, p, _ = euler_from_quat(self.base_quat)
   r_term = torch.abs(r) > 0.8
   p_term = torch.abs(p) > 0.8
   

   • 该部分代码使用四元数 self.base_quat 计算机器人基座的俯仰角（p）和横滚角（r）。如果横滚角或俯仰角的绝对值超过 0.8 弧度（约 45 度），则认为该环境需要终止并重置。

3. 高度终止条件 (z_term):

   z = self.root_states[:, 2]
   z_term = z < 0.1
   

   • 该部分代码检查机器人基座的高度 z 是否低于 0.1。如果低于该阈值，则认为机器人可能已经跌倒，环境需要终止并重置。

4. 超时终止条件 (time_out_buf):

   self.time_out_buf = self.episode_length_buf > self.max_episode_length
   

   • 该部分代码判断当前时间步是否超过了预设的最大时间步 self.max_episode_length。如果超过，则该环境的仿真步数达到上限，需要终止。

5. 终止条件的综合判断:

   self.reset_buf = termination_contact_buf | self.time_out_buf | r_term | p_term | z_term
   

   • 最后，reset_buf 是一个布尔向量，表示每个环境是否需要重置。它综合了接触力、姿态、高度和超时等条件，任何一个条件触发，都会导致该环境的重置。

check_termination 函数通过多种条件的检查（接触力、姿态、高度、超时）来决定当前仿真环境是否需要重置，以确保机器人在异常状态下能够及时终止并重新初始化，从而避免不必要的错误累积。这些终止条件是机器人仿真中常用的安全检查机制，保证仿真环境的稳定性和任务的顺利进行。

创建模拟环境 (create_sim):

设置并初始化物理仿真环境，包括地形和机器人模型等。

功能：创建仿真环境和地形。
实现：
    设定世界坐标系的上轴（up_axis_idx），这里设为 2 表示 z 轴是上轴。
    调用 self.gym.create_sim 方法创建仿真环境，这包括了设备ID、图形设备ID、物理引擎类型和仿真参数。
    根据配置中的地形类型 (mesh_type)，选择创建不同的地形。可以是平面、高度场或三角网格。
    最终调用 _create_envs 方法来创建环境中的具体实体，如机器人等。

Terrain

Terrain 类是一种地形环境配置类，主要用于创建和管理地形模拟环境。该类通过配置参数 cfg（一个 LeggedRobotCfg.terrain 类型的对象）来初始化，并设置地形的基本属性和方法。这里详细分析一下该类及其关键函数 curriculum 的实现和功能。
类初始化

    cfg: 接收一个关于地形的配置对象，用于初始化地形的各种参数。
    type: 根据配置确定地形的类型（如 'none', 'plane', 'trimesh' 等）。
    env_length 和 env_width: 环境的长度和宽度，从配置中获得。
    proportions: 根据配置中的 terrain_proportions 计算不同地形比例的累积和，用于后续地形生成中的随机选择。
    num_sub_terrains: 根据行列数计算子地形的总数。
    env_origins: 存储每个子地形在世界坐标中的起始点。
    height_field_raw: 初始化一个数组，用于存储整个环境的高度信息。

curriculum 函数

该函数用于按照课程学习（curriculum learning）的方式生成地形。课程学习在这里是指随着行列索引的增加，地形的难度逐渐增加。函数实现如下：

    random: 一个布尔值，指示是否随机生成地形。如果为 True，则地形难度和类型完全随机。
    通过双重循环遍历每一个子地形的位置。
    difficulty: 根据行索引计算当前子地形的难度，难度由0逐渐增加至1。
    choice: 根据列索引计算一个用于选择地形类型的值。
    根据 choice 和 difficulty 调用 make_terrain 函数生成具体的地形，然后将其添加到整体地形映射中。

地形生成逻辑

地形的生成基于配置中定义的比例和类型，利用 make_terrain 和其他辅助函数（如 add_roughness, gap_terrain 等）来创建具体的地形结构。这些函数基于物理和几何参数来调整地形的高度和形状，以模拟现实世界中的不同地形障碍。
总结

Terrain 类提供了一个灵活的方式来生成和管理复杂的地形环境，支持多种地形类型和难度级别，适用于机器学习和仿真领域中地形导航和路径规划的研究。通过课程学习方法生成的地形能够有效地帮助机器学习模型逐步适应不同难度的环境，有助于提高模型的泛化能力和鲁棒性。

convert_heightfield_to_trimesh_delatin

函数 convert_heightfield_to_trimesh_delatin 用于将高度场数组使用 Delatin 库转换成三角形网格，这个库在创建适合物理模拟或视觉渲染的详细地形网格方面非常有效。我们来逐步分析这个函数的实现和使用方法：

函数定义

def convert_heightfield_to_trimesh_delatin(height_field_raw, horizontal_scale, vertical_scale, max_error=0.01):

• 参数：
  • height_field_raw：原始的高度场数组，代表地形的高度数据。
  • horizontal_scale：水平缩放比例，用于调整网格的水平尺寸。
  • vertical_scale：垂直缩放比例，用于调整网格的高度。
  • max_error：在生成网格时允许的最大误差，默认值为0.01。

函数实现步骤

1. 网格初始化：

   • 使用 Delatin 类初始化一个网格。Delatin 是一个高效的库，用于将高度场数据转换为三角形网格。这里，height_field_raw 被沿第二轴（axis=1）翻转并转置（.T），这是因为 Delatin 需要以特定的格式接收高度数据。

2. 顶点数组创建：

   • 创建一个与 Delatin 生成的顶点数组同形状的新数组 vertices。这个数组将存储转换后的顶点坐标。

3. 应用缩放：

   • 将 Delatin 生成的顶点数组中的 x 和 y 坐标（即前两列）乘以 horizontal_scale 来进行缩放。
   • 将 z 坐标（即第三列）保持不变，因为垂直缩放已在 Delatin 初始化时应用。

• 返回处理后的 vertices 数组，可以用于进一步的渲染或物理模拟过程。4221682,3
• 还有mesh.triangels 8443358,3

这个函数通过对高度数据的有效处理和转换，支持复杂地形的实时模拟和可视化，是地形处理流程中的关键步骤。

_create_envs 函数

功能：创建仿真环境中的实体。
    实现：
        首先加载机器人的URDF或MJCF模型。
        为每个环境实例化机器人，包括设置刚体和关节的属性。
        调用相关的回调函数来随机化或设置动力学属性。
        最终将机器人添加到各个环境中。

重置环境 (reset_idx):

用于在特定条件下重置环境状态，例如在新的训练周期开始时。

其他辅助函数:

包括处理碰撞、设置目标位置、计算转换矩阵等工具函数。
此类的实现复杂且高度模块化，依赖于多个配置选项和环境参数来适应不同的训练和测试需求。使用时，可以通过更改配置文件来调整机器人的行为和学习任务。

_control_ik(self, dpose)：

    功能：基于给定的期望位姿变化（dpose 6144,6,1），计算逆运动学得到的臂部位置目标。
    参数：
        dpose：期望的位姿变化，包括位置和方向的变化。
    实现：使用阻尼最小二乘法解决逆运动学问题。首先计算雅可比矩阵 6144,6,6 的转置，然后结合正则化项（lambda），使用解析解求解线性方程组得到臂部关节的目标位置。

这个函数用于执行阻尼最小二乘法（Damped Least Squares，DLS）来解决机器人末端执行器的逆运动学问题。逆运动学用于计算给定末端执行器位置和姿态的关节角度。

1. 雅可比矩阵的转置 (j_eef_T)：

   • self.ee_j_eef 是末端执行器的雅可比矩阵，它描述了关节速度与末端执行器速度之间的线性映射关系。
   • j_eef_T 是 self.ee_j_eef 的转置，便于后续矩阵运算。

2. 阻尼因子 (lmbda)：

   • 为了解决雅可比矩阵可能是奇异矩阵或接近奇异的情况，使用了阻尼因子 lmbda。这里的阻尼因子是一个对角矩阵，对角线上的每个元素都是 0.0025。
   • 阻尼因子的作用是增加雅可比矩阵的对角线元素，防止求解过程中出现数值不稳定的问题。

3. 计算 A 矩阵 (A)：

   • A = J * J^T + lmbda，这里 J 是雅可比矩阵，J^T 是它的转置。这个矩阵用于稳定逆运动学解的计算。

4. 求解控制输入 (u)：

   • 通过 torch.linalg.solve(A, dpose) 求解方程 A * x = dpose，得到关节速度的解 x，然后乘以 j_eef_T 得到最终的关节速度 u。

5. 去除额外的维度 (squeeze)：

   • u.squeeze(-1) 将 u 张量的最后一维去除，使其形状符合后续操作的需要。
     6144,6

_control_ik 函数通过阻尼最小二乘法来计算给定的末端执行器位置和姿态误差下所需的关节速度。这个方法在逆运动学求解中引入了阻尼因子，增加了系统的数值稳定性，特别适用于可能存在奇异情况的机器人控制问题。

_compute_torques(self, actions)：

    功能：根据动作（actions 6144,18）计算扭矩。
    参数：
        actions：动作输入，可以是位置目标、速度目标或直接扭矩。
    实现：动作值首先根据电机强度和动作比例因子进行缩放。如果动作是位置或速度目标，则通过比例-微分（PD）控制器计算扭矩。直接扭矩则由动作值直接确定。6144,19

_reset_dofs(self, env_ids)：

    功能：重置指定环境的机器人关节状态。
    参数：
        env_ids：需要重置的环境ID列表。
    实现：随机选择关节的初始位置，并将关节速度设置为零。

_resample_ee_goal_sphere_once(self, env_ids)：
    功能：为指定的环境随机重新采样末端执行器目标位置。
    参数：
        env_ids：需要重新采样目标位置的环境ID列表。
    实现：根据配置文件中定义的范围，随机生成新的末端执行器目标位置（球坐标系）。
_get_env_origins
    环境原点的获取：这个函数主要用于获取每个环境实例的原点（origins）。
    地形的环境原点：
        如果 terrain 属性不为空，则函数返回地形的环境原点（env_origins）。地形的环境原点通常是根据地形的生成逻辑决定的，用于确定各个环境实例在三维空间中的具体位置。
    自定义原点：
        如果 terrain 属性为空但 custom_origins 属性不为空，则返回 custom_origins。custom_origins 允许用户自定义每个环境实例的原点位置。
    调用父类方法：
        如果 terrain 和 custom_origins 都为空，函数将调用父类的 _get_env_origins 方法获取默认的环境原点。
_process_rigid_shape_props
	这个函数用于处理刚体的形状属性（props）。
    地形的属性应用：
        如果 terrain 属性不为空，则函数会将地形的摩擦系数（friction）和恢复系数（restitution）应用到传入的 props 中。
        摩擦系数决定了物体与地形之间的摩擦力大小，恢复系数则决定了物体在碰撞后的弹性恢复程度。
    返回处理后的属性：函数最后返回经过处理的属性字典 props。
	该函数的设计目的是确保所有与地形交互的刚体都能够正确应用地形的物理属性。这是为了使仿真更加真实和一致，特别是在有不同摩擦系数和恢复系数的地形上操作时。
_build_viz_rotating_object：
    这个函数通常用于在仿真中构建可视化的旋转物体，用于提供视觉反馈或进行仿真中的测试。

_create_custom_origins：
    用于创建自定义的环境原点，这对于需要在特定位置初始化环境的场景非常有用。

_get_task_obs：
    获取任务相关的观测值，这些观测值通常用于智能体的决策过程。

_set_custom_origins：
    设置自定义的环境原点，以确保环境在特定的位置初始化。        

subscribe_viewer_keyboard_events

subscribe_viewer_keyboard_events 函数主要用于订阅不同键盘事件，以便在仿真过程中处理用户的输入。这些事件包括退出仿真、切换视图同步、开启自由视角模式、选择关注对象（机器人）等。

• 键盘事件订阅：
  • KEY_ESCAPE: 订阅退出事件，按下此键会退出仿真。
  • KEY_V: 订阅切换视图同步事件，按下此键会切换视图同步模式。
  • KEY_F: 订阅自由视角模式，按下此键会切换是否开启自由视角模式。
  • KEY_LEFT_BRACKET 和 KEY_RIGHT_BRACKET: 订阅切换关注对象的事件，按下这两个键可以切换当前相机关注的机器人。
  • KEY_SPACE: 订阅暂停事件，按下此键会暂停仿真。

这些键盘事件的订阅有助于在仿真过程中动态地控制仿真视图和交互。

_init_buffers

_init_buffers 函数的作用是初始化在仿真过程中用于存储观测数据、动作数据、奖励等信息的缓冲区。这些缓冲区将用于存储和管理多个环境实例中的数据，确保仿真过程中的数据能够正确流转和更新。

以下是对函数的详细分析：

1. 观测缓冲区初始化：

   self.obs_buf = torch.zeros((self.num_envs, self.num_obs), device=self.device, dtype=torch.float)
   

   • 这里初始化了一个大小为 (self.num_envs, self.num_obs) 的观测缓冲区 obs_buf，用于存储每个环境实例的观测数据。
   • self.num_envs 表示环境实例的数量，self.num_obs 表示每个环境的观测维度。
   • device 参数指定了缓冲区的存储设备（如 GPU），dtype 指定了数据类型为 float。

2. 目标观测缓冲区初始化：

   self.privileged_obs_buf = torch.zeros((self.num_envs, self.num_privileged_obs), device=self.device, dtype=torch.float)
   

   • 该缓冲区用于存储一些特权观测数据，这些数据可能不用于普通训练，但用于评估或其他目的。
   • self.num_privileged_obs 表示特权观测的维度。

3. 动作缓冲区初始化：

   self.actions = torch.zeros((self.num_envs, self.num_actions), device=self.device, dtype=torch.float)
   

   • 该缓冲区用于存储每个环境实例的动作数据。
   • self.num_actions 表示每个环境的动作维度。

4. 奖励缓冲区初始化：

   self.rew_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.float)
   

   • 该缓冲区用于存储每个环境实例的奖励值。

5. 重置标志缓冲区初始化：

   self.reset_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.long)
   

   • 该缓冲区用于标记哪些环境实例需要重置。
   • 使用 long 类型是因为可能需要标记多个状态。

6. 终止标志缓冲区初始化：

   self.terminated_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.long)
   

   • 该缓冲区用于标记哪些环境实例已经终止。

7. 截断标志缓冲区初始化：

   self.truncated_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.long)
   

   • 该缓冲区用于标记哪些环境实例由于某些条件（如达到最大步骤数）而被截断。

8. 观测标准化和奖励标准化缓冲区：

   self.obs_rms = None
   self.ret_rms = None
   

   • 这些缓冲区用于标准化观测数据和奖励值。标准化能够帮助训练过程更加稳定。

通过初始化这些缓冲区，_init_buffers 函数为仿真过程的数据流转提供了存储和管理机制。这些缓冲区将在仿真过程中不断更新，用于存储和管理不同环境实例的观测数据、动作数据、奖励等信息。

_get_noise_scale_vec

_get_noise_scale_vec 函数的作用是生成一个噪声缩放向量，用于在仿真过程中对观测数据或动作数据添加噪声，以增强模型的鲁棒性或模拟现实中的不确定性。

以下是对函数的详细分析：

1. 噪声缩放向量的初始化：

   noise_scale_vec = torch.ones(self.num_obs, device=self.device)
   

   • 这里首先初始化了一个大小为 self.num_obs 的全1向量 noise_scale_vec，其中 self.num_obs 表示观测的维度。
   • device 指定了向量存储的设备（如 GPU）。

2. 检查是否有噪声：

   if self.noise_scale_vec is not None:
       noise_scale_vec *= self.noise_scale_vec
   

   • 这里检查 self.noise_scale_vec 是否不为空（即是否配置了噪声缩放向量）。
   • 如果配置了，则将初始化的全1向量 noise_scale_vec 与 self.noise_scale_vec 相乘，以得到最终的噪声缩放向量。

3. 返回噪声缩放向量：

   return noise_scale_vec
   

   • 最终返回计算后的噪声缩放向量 noise_scale_vec。

_get_noise_scale_vec 函数用于根据配置的噪声缩放向量，生成一个用于观测或动作数据的噪声缩放向量。这对于训练过程中添加噪声、模拟现实环境中的不确定性、提升模型的泛化能力非常重要。

通过这一向量，可以在仿真过程中对观测数据或动作数据添加不同程度的噪声，从而提高模型对噪声和扰动的鲁棒性。

_get_ee_goal_spherical_center

_get_ee_goal_spherical_center 函数的作用是计算当前任务中指定的机械臂末端执行器（End Effector, EE）的目标位置，该位置以球形坐标的方式表示。

以下是对函数的详细分析：

def _get_ee_goal_spherical_center(self, env_ids, offset):
    pos = torch.clone(self.ghost_pose[env_ids, :3])
    pos[:, 0] += offset
    return pos

1. 函数参数：

   • env_ids：表示环境的索引，这个索引用来从 self.ghost_pose 中选择相应环境的位置信息。
   • offset：表示在计算末端执行器目标位置时，在 X 轴方向上的偏移量。

2. 获取当前位置：

   pos = torch.clone(self.ghost_pose[env_ids, :3])
   

   • 这里从 self.ghost_pose 中提取对应 env_ids 环境的前三个元素，即末端执行器的当前位置（X, Y, Z）。
   • torch.clone 用于创建当前位置的一个副本，避免直接修改原始数据。

3. 添加偏移量：

   pos[:, 0] += offset
   

   • 对 pos 向量的 X 轴位置（第一个元素）加上 offset，以此确定新的目标位置。

4. 返回目标位置：

   return pos
   

   • 最终返回修改后的末端执行器目标位置向量 pos。

_get_ee_goal_spherical_center 函数的主要目的是计算出末端执行器的目标位置，并且这个目标位置可以通过指定的 X 轴偏移量来调整。这个函数在机器人控制和路径规划中非常重要，特别是在多环境的仿真中，每个环境可能会有不同的目标位置。
因为在多环境的并行仿真中，每个环境都需要一个独立的目标位置。将这些位置存储在一个大小为 (num_envs, 3) 的张量中，能够便于统一管理和高效计算

_prepare_reward_function

功能概述

_prepare_reward_function 函数的主要目的是准备奖励函数列表。这些奖励函数将在仿真过程中被调用，以计算总的奖励值。函数会遍历配置文件中所有非零的奖励比例（scale），并为每一个奖励比例准备相应的函数。

代码实现步骤

1. 导入所需模块

   from legged_gym.envs.rewards.maniploco_rewards import ManipLoco_rewards 
   

   • 函数首先从legged_gym.envs.rewards.maniploco_rewards模块中导入ManipLoco_rewards类，这是一个包含各种奖励函数的容器。

2. 初始化奖励容器

   reward_contrainers = {"maniploco_rewards": ManipLoco_rewards}
   self.reward_container = reward_contrainers[self.cfg.rewards.reward_container_name](self)
   

   • reward_contrainers字典将配置文件中的reward_container_name映射到具体的奖励类，这里是ManipLoco_rewards。
   • self.reward_container则是ManipLoco_rewards类的实例，实例化时传入了当前环境对象self。

3. 筛选有效的奖励比例

   self.reward_scales = {k:v for k,v in self.reward_scales.items() if v is not None and v != 0}
   

   • 这里对self.reward_scales进行了过滤，只保留非空且非零的奖励比例。

4. 准备奖励函数列表

   self.reward_functions = []
   self.reward_names = []
   for name, scale in self.reward_scales.items():
       if name=="termination":
           continue
       self.reward_names.append(name)
       name = '_reward_' + name
       self.reward_functions.append(getattr(self.reward_container, name))
   

   • 函数遍历所有有效的奖励比例，并根据比例名称在reward_container中找到相应的函数。每个函数以_reward_为前缀命名，如_reward_<name>。
   • 这些函数被添加到self.reward_functions列表中，而它们的名称被存储在self.reward_names中。

5. 准备臂（arm）奖励函数列表

   self.arm_reward_scales = {k:v for k,v in self.arm_reward_scales.items() if v is not None and v != 0}
   
   self.arm_reward_functions = []
   self.arm_reward_names = []
   for name, scale in self.arm_reward_scales.items():
       if name=="termination":
           continue
       self.arm_reward_names.append(name)
       name = '_reward_' + name
       self.arm_reward_functions.append(getattr(self.reward_container, name))
   

   • 与上一步类似，专门为机械臂部分准备了奖励函数，存储在self.arm_reward_functions中。

6. 初始化奖励累计变量

   self.episode_sums = {name: torch.zeros(self.num_envs, dtype=torch.float, device=self.device, requires_grad=False)
                        for name in list(self.reward_scales.keys()) + list(self.arm_reward_scales.keys())}
   
   self.episode_metric_sums = {name: torch.zeros(self.num_envs, dtype=torch.float, device=self.device, requires_grad=False)
                       for name in list(self.reward_scales.keys()) + list(self.arm_reward_scales.keys())}
   

   • 为了记录每一轮仿真中的累计奖励，函数还初始化了两个字典episode_sums和episode_metric_sums。这两个字典的键分别是奖励的名称，而值则是长度为num_envs的零张量。

_prepare_reward_function 是一个关键的初始化函数，用于将配置文件中的奖励比例映射到具体的奖励计算函数。函数通过灵活的设计，可以支持多种奖励类型和不同的机械臂任务。这些奖励函数在仿真每一步中都会被调用，以计算并累积奖励值，从而影响仿真代理的学习效果。

类 LeggedRobot 分析：

LeggedRobot 类是 BaseTask 类的子类，专门为仿真中的有腿机器人环境设计。它扩展了基本任务类的功能，增加了特定于有腿机器人的方法和属性。下面详细分析这个类的主要组成部分和功能。

初始化（init）

在初始化阶段，LeggedRobot 类接收配置对象 cfg，该配置定义了环境的多个方面，如仿真参数、物理引擎、设备类型等。在调用 super().init() 从 BaseTask 继承基础的初始化后，它执行以下特定于有腿机器人的初始化操作：

设置相机视角（如果不是无头模式）。
初始化奖励函数。
标记初始化完成。

步骤（step）

step 方法是机器人仿真中的一个核心方法，它接收动作作为输入，执行以下操作：

应用动作到仿真。
进行物理仿真。
调用 post_physics_step 方法处理物理仿真后的数据更新。

post_physics_step

此方法用于在每次物理仿真步骤后更新观察、奖励和终止条件等：

刷新各种状态和观察缓冲区。
检查是否需要重置环境。
计算奖励和观察。

重置（reset 和 reset_idx）

reset 方法重置所有环境到初始状态。
reset_idx 方法允许重置指定的环境索引。

创建仿真（create_sim）

该方法负责创建仿真环境，包括地形和机器人模型的加载和设置。

奖励函数

根据配置中定义的奖励规模，LeggedRobot 类使用多种奖励函数来评估机器人的性能，例如速度追踪、姿态保持和接触力管理等。

观察和状态管理

管理和更新机器人的状态，如关节位置、速度和接触力等。
提供观察结果给强化学习算法用于训练。

总体而言，LeggedRobot 类在 BaseTask 的基础上增加了对有腿机器人特有的动态环境管理和仿真控制的支持，使其成为实现高级机器人训练仿真的强大工具。

函数 _reset_root_states 是 ManipLoco 类中的一个方法，它用于重置特定环境中的根状态，包括机器人和箱子的初始位置、姿态和速度。这些状态的设置通常用于在仿真中重新初始化某些环境，从而开始新的仿真轮次。让我们逐步解析这个函数的具体实现。

_reset_root_states

1. 初始化根状态:

   • self.root_states[env_ids] = self.base_init_state：首先，将 env_ids 对应环境的根状态设置为初始状态。这包括位置、旋转、线速度和角速度。
   • self.root_states[env_ids, :3] += self.env_origins[env_ids]：在此基础上，更新根状态的位置，将其调整到相应的环境起始位置 self.env_origins[env_ids]。
   • self.root_states[env_ids, :2] += torch_rand_float(...)：在x和y轴上引入小的随机扰动，使得初始位置在一定范围内随机分布。这有助于增加仿真的多样性。

2. 设置箱子的位置:

   • self.box_root_state[env_ids, 0] = self.env_origins[env_ids, 0] + 2：设置箱子在x轴上的初始位置，比机器人偏移2米。
   • self.box_root_state[env_ids, 1] = self.env_origins[env_ids, 1]：y轴位置与环境起始位置一致。
   • self.box_root_state[env_ids, 2] = self.env_origins[env_ids, 2] + self.cfg.box.box_env_origins_z：z轴位置基于配置中的偏移量进行设置。

3. 随机化初始朝向:

   • rand_yaw = self.cfg.init_state.rand_yaw_range*torch_rand_float(...)：生成一个在设定范围内的随机yaw角。
   • quat = quat_from_euler_xyz(...)：将这个yaw角转换为四元数表示，并应用到环境中的根状态。

4. 随机化初始速度:

   • self.root_states[env_ids, 7:13] = torch_rand_float(...)：为环境中的根状态随机生成初始线速度和角速度。这些速度在设定的范围内随机分布。

5. 更新仿真中的根状态:

   • self.gym.set_actor_root_state_tensor(...) 和 self.gym.refresh_actor_root_state_tensor(...)：将更新后的根状态应用到仿真中，使其在下一次仿真步时生效。

这个函数的主要功能是通过设置初始位置、朝向和速度，重置仿真环境中的机器人和物体的根状态。通过引入随机化，它使得每次仿真开始时都有一些差异，增加了仿真训练的多样性。这对于强化学习任务中的泛化能力至关重要。

在 ManipLoco 类的 _reset_root_states 函数中，box 通常指的是仿真环境中的一个物体，它可能是一个用于测试或训练任务的物理对象。在很多仿真环境中，箱子（box）被用来模拟场景中的障碍物、目标物或其他需要与机器人交互的物体。

在代码中，self.box_root_state 代表了这些箱子的根状态信息，其中包括箱子的初始位置和姿态。通过 _reset_root_states 函数中的代码，你可以看到箱子的初始位置和姿态是如何被设置的。例如，箱子的x轴位置被设置为比机器人的环境起始位置多2米，而z轴位置则根据配置文件中的参数 self.cfg.box.box_env_origins_z 进行了偏移。

在这个特定的环境中，box 很可能是一个用于与机器人进行交互的物体，例如机器人需要避开或搬运的箱子。通过设置箱子的初始位置和姿态，仿真环境得以正确初始化，以便于接下来的任务训练或测试。

_resample_commands 方法分析

_resample_commands 方法的主要功能是为指定的环境（由 env_ids 指定）随机生成新的运动指令（commands）。这些指令通常包括机器人的线速度和角速度。

• env_ids: 表示需要为哪些环境生成新指令的环境ID列表。

1. 检查远程操作模式 (teleop_mode):

   • 如果当前配置中启用了 teleop_mode，则直接返回，不生成新的指令。这可能是因为在远程操作模式下，指令由外部控制输入，而不是随机生成。

2. 根据 global_steps 选择速度范围:

   • self.global_steps < 5000 * 24：如果全局步骤数少于 5000 * 24，则仅生成向前运动的指令（线速度 x 方向）。
   • 如果步骤数大于等于 5000 * 24，则在指定的 command_ranges 范围内生成完整的随机线速度指令。

3. 生成新的指令:

   • self.commands[env_ids, 0]：为指定的环境生成线速度 x 方向的随机指令。
   • self.commands[env_ids, 1]：将 y 方向的线速度设为 0。
   • self.commands[env_ids, 2]：为指定的环境生成随机的角速度（绕 z 轴的旋转速度）指令。

4. 处理小指令:

   • 最后一步是将小于指定阈值的指令设置为 0，以忽略不重要的微小指令。这通过比较生成的指令和配置中的阈值 lin_vel_x_clip 和 ang_vel_yaw_clip 实现。

这个方法通过为每个环境生成随机的运动指令，来模拟机器人在不同情况下的行为。这些指令是以一定的概率生成的，且在训练早期可能更侧重于特定方向（如前进方向）的指令生成。

_resample_ee_goal

• 目标: 重新采样指定环境的末端执行器的目标位置，并将其转换为笛卡尔坐标。

• 使用场景: 这个方法通常在初始化或者运行过程中需要更新目标位置时调用。

• env_ids：一个包含环境 ID 的列表，用于指定哪些环境需要重新采样目标位置。

• is_init：一个布尔值，指示当前是否处于初始化阶段。如果是初始化阶段，将使用预先定义的起始和目标位置。

1. 远程操作模式检查 (teleop_mode)：

   • 如果当前处于远程操作模式并且是初始化阶段，直接使用初始化的目标位置，并退出函数。
   • 如果处于远程操作模式且不是初始化阶段，也直接退出函数。

2. 采样目标位置 (ee_goal)：

   • 如果 env_ids 长度大于 0，开始采样新的目标位置。
   • 如果是初始化阶段，将末端执行器的目标方向增量（ee_goal_orn_delta_rpy）设为 0，并将目标球面坐标（ee_goal_sphere）设为预定义的初始位置。
   • 如果不是初始化阶段，首先对目标的方位角进行一次随机采样，然后尝试重新采样目标的球面坐标（最多 10 次），以确保目标不会与其他物体发生碰撞。

3. 碰撞检测：

   • 使用 _collision_check 函数进行碰撞检测。如果检测到碰撞，继续重新采样，直到找到一个无碰撞的目标位置或达到最大尝试次数（10 次）。

4. 更新目标位置：

   • 将最终确定的目标位置从球面坐标转换为笛卡尔坐标，并存储在 ee_goal_cart 中，同时将计时器 goal_timer 置为 0。

• 远程操作模式 (teleop_mode)：在这种模式下，目标位置不会被随机采样，而是使用预定义的初始位置。

• 碰撞检测：确保重新采样的目标位置不会与环境中的其他物体发生碰撞，这是仿真和实际操作中都非常关键的一步。

• 该方法确保了末端执行器的目标位置是合理的，并且不会引起潜在的碰撞问题，尤其在复杂环境中非常重要。

• 通过多次采样和碰撞检测，这个函数能够生成一个有效的目标位置，适应不同的环境需求。

类 BaseTask 分析：

BaseTask 是一个为仿真任务提供基础框架的抽象基类。它定义了任务需要的一些基本操作和属性，为特定的仿真任务如 LeggedRobot 提供了通用的方法和接口。

初始化（init）

在 BaseTask 的初始化方法中，主要进行以下几个步骤：

接收和存储仿真环境的配置。
初始化物理引擎和仿真参数。
设置仿真设备（CPU或GPU）。
准备环境的无头模式（不显示图形界面的模式）。

在 BaseTask 类的构造函数 __init__ 中，有一些关键的初始化步骤。下面重点分析导入了 subscribe_viewer_keyboard_events 的部分：

• 仿真设备设置：
  根据传入的 sim_device 参数，设置了仿真设备为 GPU 或 CPU。同时，根据配置决定是否在 GPU 上进行仿真图像的渲染。

• 环境和缓冲区初始化：
  设置了仿真环境的参数，包括环境的数量、观测的维度、行动的维度等，并分配了用于存储观测、奖励和重置标志的缓冲区。

• 仿真环境和视图创建：
  调用了 create_sim 函数以创建仿真环境，并根据 headless 参数决定是否创建一个仿真视图。如果 headless 为 False，则创建视图并订阅键盘事件。

• 键盘事件订阅：
  在视图创建后，调用 subscribe_viewer_keyboard_events 函数，订阅前述的各类键盘事件，允许用户在仿真过程中进行交互。

通过这些步骤，BaseTask 类完成了基础的仿真环境初始化和用户交互的设置，为具体任务的执行打下了基础。

创建仿真环境（create_sim）

此方法负责根据传入的参数和配置创建仿真环境。具体的实现在子类中完成，BaseTask 类中通常只提供接口。

重置环境（reset）

reset 方法用于将仿真环境重置到初始状态。具体的重置行为（如重置机器人位置或环境状态）在子类中具体定义。

步骤执行（step）

step 方法定义了在每个仿真步骤中执行的动作，如应用动作、执行物理仿真步骤等。该方法需要在子类中具体实现动作的应用和仿真的更新。

结束条件（is_done）

is_done 方法用于判断仿真是否达到结束条件。这可能包括任务完成、发生碰撞或达到最大步数等。具体的判断逻辑需在子类中实现。

奖励计算（compute_reward）

这是一个抽象方法，需要在子类中定义具体的奖励计算逻辑。奖励是评估仿真中代理的行为表现的关键指标。

状态和观察获取（get_observations）

获取当前环境状态的方法，通常返回代理需要的观察数组。这些观察数据用于训练强化学习模型。

渲染环境（render）

如果仿真不是在无头模式下运行，render 方法用于生成和更新仿真的可视化。

总体来说，BaseTask 提供了仿真任务需要的基本结构和方法，而具体的任务逻辑（如机器人控制、奖励计算和结束条件）需要在继承了 BaseTask 的子类中实现。这种设计允许高度的可扩展性和复用性，便于开发和测试不同类型的仿真任务。

runner

所以上面的过程返回了ManipLoco实例env，以及env_cfg

make_alg_runner 函数分析

make_alg_runner 函数是 TaskRegistry 类中的一个方法，用于创建训练算法实例。该函数接受环境对象 env、任务名称 name、命令行参数 args、训练配置 train_cfg 和日志路径 log_root 等参数，最终返回一个用于强化学习的训练算法实例（如 PPO 算法）及对应的训练配置。

• env：需要训练的环境实例。
• name：任务名称。如果未提供 train_cfg，则使用 name 从注册表中加载相应的配置文件。
• args：命令行参数，如果为空，则会调用 get_args() 获取默认参数。
• train_cfg：训练配置文件。如果未提供，则根据 name 来获取相应的配置文件。
• log_root：日志目录，默认为 “default”，即日志将被保存到 <path_to_LEGGED_GYM>/logs/<experiment_name> 目录中。

1. 参数解析：

   • 如果 args 为 None，则调用 get_args() 获取默认命令行参数。
   • 如果未提供 train_cfg，但提供了 name，则从任务注册表中获取相应的训练配置文件。
   • 如果同时提供了 name 和 train_cfg，则忽略 name，并打印警告信息。

2. 配置文件的覆盖：

   • 调用 update_cfg_from_args 函数，用命令行参数覆盖默认配置文件中的设置。
   • checkpoint 和 log_dir 会从 args 和 train_cfg 中获取或初始化。

3. 选择算法实例：

   • 根据环境名称选择合适的算法实例：
     • 如果环境名称为 “b1z1_pick”，则选择 OnPolicyRunnerHRL，否则选择 OnPolicyRunner。
   • 初始化算法实例，并将 train_cfg 转换为字典形式传递给算法。

4. 模型恢复：

   • 如果需要恢复训练（即 resume=True），则从 log_root 中指定的路径加载先前的模型。
   • 设置算法的迭代次数 checkpoint，并根据配置决定是否从最后一次标准偏差 std 开始训练。

5. 返回值：

   • 如果 kwargs 中包含 return_log_dir，则返回 runner、train_cfg、checkpoint 以及恢复路径的上一级目录，否则返回 runner、train_cfg 和 checkpoint。

• 灵活性：函数支持通过命令行参数覆盖默认的配置文件，适应不同的实验需求。
• 模型恢复：能够从上次中断的地方继续训练，这是长时间训练任务中的关键功能。
• 任务特化：根据任务名称选择不同的算法实现，适应不同任务的需求。

这个函数在强化学习训练流程中起到核心作用，通过结合环境和训练配置，创建并管理一个算法实例，使其能够高效地在指定环境中进行训练。

分析函数调用链（OnPolicyRunner -> PPO -> RolloutStorage）

在分析调用链时，我们从 OnPolicyRunner 类开始，然后深入到 PPO 类，最终到达 RolloutStorage 类。这些类之间的调用链帮助我们理解整个强化学习过程的架构。

1. OnPolicyRunner 类中的 __init__ 方法

• 文件：on_policy_runner.py

class OnPolicyRunner:

    def __init__(self,
                 env: VecEnv,
                 train_cfg,
                 log_dir=None,
                 device='cpu'):
        ...
        self.alg: PPO = alg_class(actor_critic, device=self.device, **self.alg_cfg)
        ...
        self.alg.init_storage(self.env.num_envs, self.num_steps_per_env, [self.env.num_obs], [self.env.num_privileged_obs], [self.env.num_actions])
        ...

 _,_=self.env.reset()

调用ManipLoco的reset→step

• 分析：
  OnPolicyRunner 是强化学习训练的核心管理类。它的 __init__ 方法中创建了 PPO 算法的实例 self.alg，并调用了 self.alg.init_storage 来初始化存储。这个过程主要负责设置 PPO 算法中的存储部分，即 RolloutStorage，用于存储环境交互的数据。

2. PPO 类中的 init_storage 方法

• 文件：ppo.py

class PPO:
    ...
    def init_storage(self, num_envs, num_transitions_per_env, actor_obs_shape, critic_obs_shape, action_shape):
        self.storage = RolloutStorage(num_envs, num_transitions_per_env, actor_obs_shape, critic_obs_shape, action_shape, self.device)
    ...

• 分析：
  PPO 类的 init_storage 方法用于初始化 RolloutStorage 对象。这个对象用于在多个环境中收集样本数据，包括观察值、动作、奖励等。在强化学习的更新步骤中，这些数据会被用来计算策略梯度并更新模型。

3. RolloutStorage 类

• 文件：rollout_storage.py

class RolloutStorage:
    def __init__(self, num_envs, num_transitions_per_env, actor_obs_shape, critic_obs_shape, action_shape, device):
        ...

• 分析：
  RolloutStorage 类负责管理在多个环境中的交互数据存储。初始化时，会为每个环境分配存储空间，用于存储观察值、动作、奖励等信息。这些数据在后续的策略优化中非常关键，确保 PPO 算法能够高效地学习。

通过分析 OnPolicyRunner 到 PPO，再到 RolloutStorage 的调用链，可以看出这一系列操作是强化学习训练过程中的核心步骤。OnPolicyRunner 类负责管理整个训练过程，PPO 类负责具体的算法实现，而 RolloutStorage 则用于管理和存储环境交互数据。这些类相互协作，构建了一个完整的强化学习训练框架。

ManipLoco_rewards类

1. __init__ 函数

• 作用：初始化 ManipLoco_rewards 类，并接受一个环境 env 作为参数。
• 运行方式：这个函数主要是将传入的环境参数 env 存储在类的实例变量中，以便在后续的函数中可以访问和操作。
• 为什么这样运行：初始化函数是面向对象编程的基础，它为类的实例设置初始状态。在这个类中，env 是与机器人仿真环境相关的对象，很多奖励函数都需要使用它。

2. load_env 函数

• 作用：用于在运行时更换环境。
• 运行方式：将传入的新环境赋值给类的实例变量 env。
• 为什么这样运行：如果在训练过程中需要更换环境，例如切换不同的任务场景，使用这个函数可以动态地替换环境。

3. _reward_tracking_ee_sphere 函数

• 作用：计算末端执行器（End Effector, EE）在球坐标系中的误差并生成奖励。
• 运行方式：
  1. 使用 quat_rotate_inverse 函数将 EE 的位置从世界坐标系转换到局部坐标系。
  2. 使用 cart2sphere 将笛卡尔坐标转换为球坐标。
  3. 计算 EE 当前球坐标与目标球坐标之间的误差，并使用高斯函数 (torch.exp) 对误差进行处理，生成奖励。
• 为什么这样运行：这个奖励函数主要用于引导机器人末端执行器准确地跟踪球坐标系中的目标位置。通过使用高斯函数，可以确保奖励随着误差的减少而增加，从而鼓励更精确的运动。

4. _reward_tracking_ee_world 函数

• 作用：计算末端执行器在世界坐标系中的误差并生成奖励。
• 运行方式：
  1. 直接计算 EE 在世界坐标系中的位置与目标位置的误差。
  2. 使用高斯函数 (torch.exp) 对误差进行处理，生成奖励。
• 为什么这样运行：与球坐标系不同，这个奖励函数直接在世界坐标系中计算误差，适用于需要机器人精确定位到特定世界坐标的任务。

5. _reward_tracking_ee_sphere_walking 函数

• 作用：在机器人行走时计算末端执行器的球坐标跟踪奖励。
• 运行方式：
  1. 调用 _reward_tracking_ee_sphere 计算球坐标系下的跟踪奖励。
  2. 通过 self.env._get_walking_cmd_mask() 获取行走状态的掩码，并将行走状态以外的奖励置为0。
• 为什么这样运行：该函数旨在奖励机器人在行走状态下的末端执行器跟踪行为，通过掩码操作确保只有在行走状态下才会获得奖励。

6. _reward_tracking_ee_sphere_standing 函数

• 作用：在机器人站立时计算末端执行器的球坐标跟踪奖励。
• 运行方式：
  1. 调用 _reward_tracking_ee_sphere 计算球坐标系下的跟踪奖励。
  2. 通过 self.env._get_walking_cmd_mask() 获取行走状态的掩码，并将站立状态下的奖励置为0。
• 为什么这样运行：与行走状态的奖励相对，该函数用于在机器人站立时的末端执行器跟踪行为，确保只有在站立状态下才会获得奖励。

7. _reward_tracking_ee_cart 函数

• 作用：计算末端执行器在笛卡尔坐标系下的误差并生成奖励。
• 运行方式：
  1. 计算目标末端执行器位置（目标为球坐标的中心）。
  2. 计算EE当前位置与目标位置的误差。
  3. 使用高斯函数对误差进行处理，生成奖励。
• 为什么这样运行：笛卡尔坐标系适用于许多工业应用场景，这种方式直接鼓励机器人尽可能接近目标位置。

8. _reward_tracking_ee_orn 函数

• 作用：计算末端执行器的方位误差并生成奖励。
• 运行方式：
  1. 通过 euler_from_quat 将四元数表示的方位转换为欧拉角。
  2. 计算方位误差，并使用高斯函数生成奖励。
• 为什么这样运行：方位控制对于许多机器人任务非常重要，特别是需要精确的工具操作时。这个奖励函数用于鼓励精确的方位控制。

9. _reward_arm_energy_abs_sum 函数

• 作用：计算机器臂的能量消耗并生成奖励。
• 运行方式：
  1. 计算机器臂的扭矩与关节速度的绝对值乘积，并求和。
  2. 返回能量消耗值作为奖励。
• 为什么这样运行：能量消耗是机器人操作效率的一个重要指标，这个奖励函数鼓励节能操作。

10. _reward_tracking_ee_orn_ry 函数

- **作用**：计算末端执行器绕 Y 轴的方位误差并生成奖励。
- **运行方式**：
  1. 提取 EE 的欧拉角并计算绕 Y 轴的误差。
  2. 使用高斯函数对误差进行处理，生成奖励。
- **为什么这样运行**：这个函数特别关注末端执行器的俯仰和偏航角度，适用于需要精确控制特定轴旋转的任务。

11. _reward_hip_action_l2 函数

- **作用**：计算髋关节动作的L2范数并生成奖励。
- **运行方式**：
  1. 计算髋关节动作（即关节角速度）的平方和。
  2. 返回L2范数作为奖励。
- **为什么这样运行**：通过对髋关节动作的L2范数进行奖励或惩罚，鼓励或抑制过度或不适当的动作。

12. _reward_leg_energy_abs_sum 函数

- **作用**：计算机器人腿部能量消耗的绝对值总和并生成奖励。
- **运行方式**：
  1. 计算机器人腿部扭矩与关节速度的绝对值乘积，并求和。
  2. 返回能量消耗值作为奖励。
- **为什么这样运行**：类似于机器臂的能量消耗，这个函数鼓励腿部的节能运动。

13. _reward_leg_energy_sum_abs 函数

- **作用**：计算机器人腿部能量消耗的总和并生成奖励。
- **运行方式**：
  1. 计算机器人腿部扭矩与关节速度的乘积，并求和。
  2. 返回能量消耗值作为奖励。
- **为什么这样运行**：与上一个函数类似，只是这里计算的是总能量消耗。

14. _reward_leg_action_l2 函数

- **作用**：计算机器人腿部动作的L2范数并生成奖励。
- **运行方式**：
  1. 计算腿部动作的平方和。
  2. 返回L2范数作为奖励。
- **为什么这样运行**：通过对腿部动作的L2范数进行奖励或惩罚，鼓励更稳定的运动。

15. _reward_leg_energy 函数

- **作用**：计算腿部关节的能量消耗并生成奖励。
- **运行方式**：
  1. 计算腿部关节的扭矩与关节速度的乘积。
  2. 返回能量消耗值作为奖励。
- **为什么这样运行**：这个函数旨在评估腿部关节在特定运动中的能量效率。

16. _reward_tracking_lin_vel 函数

- **作用**：用于跟踪机器人线速度命令的奖励函数。
- **运行方式**：
  1. 计算命令线速度与实际线速度的误差平方和。
  2. 使用高斯函数对误差进行

处理，生成奖励。
- 为什么这样运行：鼓励机器人精确地执行线速度命令，适用于需要精确速度控制的任务。

17. _reward_tracking_lin_vel_x_l1 函数

- **作用**：用于跟踪机器人X轴线速度命令的奖励函数，采用L1范数。
- **运行方式**：
  1. 计算X轴速度命令与实际X轴速度的绝对误差。
  2. 通过与命令速度进行对比，生成奖励。
- **为什么这样运行**：L1范数通常用于衡量稀疏性，这里通过对误差进行惩罚来鼓励准确跟踪。

18. _reward_tracking_lin_vel_x_exp 函数

- **作用**：用于跟踪机器人X轴线速度命令的奖励函数，采用指数函数。
- **运行方式**：
  1. 计算X轴速度命令与实际X轴速度的绝对误差。
  2. 使用高斯函数对误差进行处理，生成奖励。
- **为什么这样运行**：指数函数使得随着误差的减小，奖励迅速增大，鼓励更精确的速度控制。

19. _reward_tracking_ang_vel_yaw_l1 函数

- **作用**：用于跟踪机器人绕Z轴的角速度命令的奖励函数，采用L1范数。
- **运行方式**：
  1. 计算绕Z轴的角速度命令与实际角速度的绝对误差。
  2. 通过与命令速度进行对比，生成奖励。
- **为什么这样运行**：用于鼓励机器人在旋转运动中的准确性。

20. _reward_tracking_ang_vel_yaw_exp 函数

- **作用**：用于跟踪机器人绕Z轴的角速度命令的奖励函数，采用指数函数。
- **运行方式**：
  1. 计算绕Z轴的角速度命令与实际角速度的绝对误差。
  2. 使用高斯函数对误差进行处理，生成奖励。
- **为什么这样运行**：同样，通过指数函数快速增加奖励，鼓励精确的旋转控制。

21. _reward_tracking_lin_vel_y_l2 函数

- **作用**：用于跟踪机器人Y轴线速度命令的奖励函数，采用L2范数。
- **运行方式**：
  1. 计算Y轴速度命令与实际Y轴速度的误差平方和。
  2. 返回L2范数作为奖励。
- **为什么这样运行**：L2范数可以惩罚较大的偏差，鼓励精确的速度跟踪。

22. _reward_tracking_lin_vel_z_l2 函数

- **作用**：用于跟踪机器人Z轴线速度命令的奖励函数，采用L2范数。
- **运行方式**：
  1. 计算Z轴速度命令与实际Z轴速度的误差平方和。
  2. 返回L2范数作为奖励。
- **为什么这样运行**：这个函数用于控制机器人的Z轴运动，特别是在需要垂直方向精确运动的任务中。

23. _reward_survive 函数

- **作用**：用于鼓励机器人的生存，通常是给与固定的奖励。
- **运行方式**：
  1. 对于每个环境，返回固定的奖励值。
- **为什么这样运行**：在强化学习中，生存奖励用于确保机器人能够继续运行下去，而不会因无奖励而陷入困境。

24. _reward_foot_contacts_z 函数

- **作用**：计算脚部接触力的平方和并生成奖励。
- **运行方式**：
  1. 计算每个脚部在Z轴上的接触力，并求平方和。
- **为什么这样运行**：鼓励合理的接触力，以避免过度的碰撞或滑动。

25. _reward_torques 函数

- **作用**：惩罚机器人的扭矩大小。
- **运行方式**：
  1. 计算每个关节的扭矩平方和，并作为惩罚值。
- **为什么这样运行**：减少不必要的扭矩输出，有助于提高能量效率和减少磨损。

26. _reward_energy_square 函数

- **作用**：计算机器人的能量平方和并生成奖励。
- **运行方式**：
  1. 计算扭矩与关节速度的平方和，并作为惩罚值。
- **为什么这样运行**：这种方法用于减少整体能耗，特别是在长时间运行中。

27. _reward_tracking_lin_vel_y 函数

- **作用**：跟踪机器人Y轴线速度命令的奖励函数。
- **运行方式**：
  1. 计算Y轴线速度命令与实际线速度的误差平方和。
  2. 使用高斯函数对误差进行处理，生成奖励。
- **为什么这样运行**：保证机器人能够在Y轴方向上精确地跟随命令。

28. _reward_lin_vel_z 函数

- **作用**：惩罚机器人Z轴速度的偏离。
- **运行方式**：
  1. 计算Z轴速度的平方和。
  2. 返回该值作为惩罚。
- **为什么这样运行**：通常用于保持机器人的Z轴稳定，特别是在不需要上下运动的任务中。

29. _reward_ang_vel_xy 函数

- **作用**：惩罚机器人绕X和Y轴的角速度。
- **运行方式**：
  1. 计算X和Y轴角速度的平方和。
  2. 返回该值作为惩罚。
- **为什么这样运行**：通过控制机器人的旋转运动，保持其姿态稳定。

30. _reward_tracking_ang_vel 函数

- **作用**：跟踪机器人角速度命令的奖励函数。
- **运行方式**：
  1. 计算命令角速度与实际角速度的误差平方和。
  2. 使用高斯函数对误差进行处理，生成奖励。
- **为什么这样运行**：确保机器人能够精确地执行给定的旋转命令。

31. _reward_work 函数

- **作用**：计算机器人腿部的做功并生成奖励。
- **运行方式**：
  1. 计算扭矩与关节速度的乘积，得到做功。
  2. 对做功取绝对值并求和，作为奖励。
- **为什么这样运行**：通过奖励做功的绝对值，鼓励更高效的运动。

32. _reward_dof_acc 函数

- **作用**：惩罚机器人的关节加速度。
- **运行方式**：
  1. 计算当前关节速度与前一时刻的差异，得到加速度。
  2. 计算加速度的平方和，作为惩罚。
- **为什么这样运行**：减少关节的突然加速，有助于延长机器人的寿命并提高其平稳性。

33. _reward_action_rate 函数

- **作用**：惩罚机器人的动作变化率。
- **运行方式**：
  1. 计算当前动作与前一时刻动作的差异。
  2. 计算差异的平方和，作为惩罚。
- **为什么这样运行**：保持动作的连续性和稳定性，有助于减少过度调整带来的能耗和机械磨损。

34. _reward_dof_pos_limits 函数

- **作用**：惩罚机器人关节超出位置限制的情况。
- **运行方式**：
  1. 对于每个关节，如果其位置超出了规定的上下限，则计算其偏差。
  2. 将所有超出限位的偏差求和，作为惩罚。
- **为什么这样运行**：保证关节在安全范围内运动，避免机械故障或损坏。

35. _reward_delta_torques 函数

- **作用**：惩罚机器人的扭矩变化。
- **运行方式**：
  1. 计算当前扭矩与前一时刻扭矩的差异。
  2. 计算差异的平方和，作为惩罚。
- **为什么这样运行**：减少扭矩的剧烈变化，平稳控制机器人运动。

36. **_reward_collision

函数**
- 作用：惩罚机器人与环境的碰撞。
- 运行方式：
1. 计算机器人与环境的接触力。
2. 对接触力大于一定阈值的部分进行惩罚。
- 为什么这样运行：避免不必要的碰撞，保护机器人和周围环境。

37. _reward_stand_still 函数

- **作用**：在机器人命令为零时惩罚任何运动。
- **运行方式**：
  1. 计算当前关节位置与默认关节位置的误差。
  2. 使用高斯函数对误差进行处理，生成奖励。
- **为什么这样运行**：确保机器人在命令为零时保持静止，避免不必要的动作。

38. _reward_walking_dof 函数

- **作用**：在机器人行走时惩罚关节偏差。
- **运行方式**：
  1. 计算当前关节位置与默认关节位置的误差。
  2. 使用高斯函数对误差进行处理，生成奖励。
- **为什么这样运行**：鼓励机器人在行走过程中保持关节的稳定性。

39. _reward_hip_pos 函数

- **作用**：计算髋关节位置偏差并生成惩罚。
- **运行方式**：
  1. 计算当前髋关节位置与默认位置的平方误差。
  2. 返回误差作为惩罚。
- **为什么这样运行**：保证髋关节在运动过程中保持在合理的范围内，避免过度的摆动或偏离。

40. _reward_feet_jerk 函数

- **作用**：惩罚脚部的突然加速度变化。
- **运行方式**：
  1. 计算当前接触力与前一时刻的差异。
  2. 对于早期的运动（例如前50步），不进行惩罚。
- **为什么这样运行**：减少脚部的突然运动，有助于实现平稳行走。

41. _reward_alive 函数

- **作用**：固定奖励，用于鼓励机器人保持活跃状态。
- **运行方式**：
  1. 返回固定的奖励值。
- **为什么这样运行**：与`_reward_survive` 类似，确保机器人不会因为无奖励而陷入停滞。

42. _reward_feet_drag 函数

- **作用**：惩罚脚部的拖拽行为。
- **运行方式**：
  1. 计算脚部在接触时的速度。
  2. 计算脚部接触时的速度平方和，作为惩罚。
- **为什么这样运行**：减少脚部拖拽，有助于保持行走的自然性和效率。

43. _reward_feet_contact_forces 函数

- **作用**：惩罚脚部接触力过大。
- **运行方式**：
  1. 计算每个脚部的接触力，并与最大接触力阈值进行比较。
  2. 对超过阈值的部分进行惩罚。
- **为什么这样运行**：避免脚部接触力过大，防止机器人因过大的接触力而损坏或不稳定。

44. _reward_orientation 函数

- **作用**：惩罚机器人基座姿态的偏离。
- **运行方式**：
  1. 计算基座姿态的重力投影在XY平面的平方和。
  2. 返回该值作为惩罚。
- **为什么这样运行**：确保机器人在运动过程中保持稳定的姿态，避免倾斜或失衡。

45. _reward_roll 函数

- **作用**：惩罚机器人基座绕X轴的滚转角度偏差。
- **运行方式**：
  1. 计算滚转角度的绝对值。
  2. 返回该值作为惩罚。
- **为什么这样运行**：通过控制滚转角度，保持机器人在运动过程中的平稳性。

46. _reward_base_height 函数

- **作用**：惩罚机器人基座高度偏离目标高度。
- **运行方式**：
  1. 计算基座高度与目标高度的差异。
  2. 返回差异的绝对值作为惩罚。
- **为什么这样运行**：确保机器人在不同地形或任务中的稳定性，防止基座高度过高或过低。

47. _reward_orientation_walking 函数

- **作用**：在行走过程中惩罚基座姿态的偏离。
- **运行方式**：
  1. 调用 `_reward_orientation` 函数计算姿态偏差。
  2. 如果机器人处于行走状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：确保机器人在行走过程中保持稳定的姿态，防止倾斜或失衡。

48. _reward_orientation_standing 函数

- **作用**：在站立过程中惩罚基座姿态的偏离。
- **运行方式**：
  1. 调用 `_reward_orientation` 函数计算姿态偏差。
  2. 如果机器人处于站立状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：类似于行走状态下的惩罚，但专注于机器人站立时的姿态控制。

49. _reward_torques_walking 函数

- **作用**：在行走过程中惩罚扭矩的使用。
- **运行方式**：
  1. 调用 `_reward_torques` 函数计算扭矩惩罚。
  2. 如果机器人处于行走状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：确保机器人在行走过程中尽量减少能量消耗，提高行走效率。

50. _reward_torques_standing 函数

- **作用**：在站立过程中惩罚扭矩的使用。
- **运行方式**：
  1. 调用 `_reward_torques` 函数计算扭矩惩罚。
  2. 如果机器人处于站立状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：类似于行走状态下的惩罚，但专注于机器人站立时的能量消耗。

51. _reward_energy_square_walking 函数

- **作用**：在行走过程中惩罚能量的平方和。
- **运行方式**：
  1. 调用 `_reward_energy_square` 函数计算能量惩罚。
  2. 如果机器人处于行走状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：减少行走过程中的能量浪费，提高效率。

52. _reward_energy_square_standing 函数

- **作用**：在站立过程中惩罚能量的平方和。
- **运行方式**：
  1. 调用 `_reward_energy_square` 函数计算能量惩罚。
  2. 如果机器人处于站立状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：类似于行走状态下的惩罚，但专注于机器人站立时的能量消耗。

53. _reward_base_height_walking 函数

- **作用**：在行走过程中惩罚基座高度的偏离。
- **运行方式**：
  1. 调用 `_reward_base_height` 函数计算基座高度偏差。
  2. 如果机器人处于行走状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：确保机器人在行走过程中保持稳定的基座高度。

54. _reward_base_height_standing 函数

- **作用**：在站立过程中惩罚基座高度的偏离。
- **运行方式**：
  1. 调用 `_reward_base_height` 函数计算基座高度偏差。
  2. 如果机器人处于站立状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：类似于行走状态下的惩罚，但专注于机器人站立时的基座稳定性。

55. _reward_dof_default_pos 函数

- **作用**：惩罚关节位置偏离默认位置的情况。

• 运行方式：
  1. 计算关节位置与默认位置的误差。
  2. 使用高斯函数对误差进行处理，生成奖励。
  • 为什么这样运行：鼓励关节回到默认位置，避免偏差导致的损坏或不稳定。

56. _reward_dof_error 函数

- **作用**：惩罚关节位置误差。
- **运行方式**：
  1. 计算关节位置与目标位置的平方误差。
  2. 返回误差作为惩罚。
- **为什么这样运行**：减少关节位置的偏差，确保准确执行动作。

57. _reward_tracking_lin_vel_max 函数

- **作用**：跟踪机器人最大线速度的奖励函数。
- **运行方式**：
  1. 计算命令速度与实际速度的最大值。
  2. 对零命令的情况进行特殊处理，返回奖励。
- **为什么这样运行**：确保机器人能够以最大效率跟随线速度命令。

58. _reward_penalty_lin_vel_y 函数

- **作用**：惩罚机器人在Y轴方向的线速度偏差。
- **运行方式**：
  1. 计算Y轴速度的绝对值。
  2. 对特定条件下的情况不应用惩罚。
- **为什么这样运行**：控制机器人在Y轴方向的运动，避免过度偏移。

59. _reward_tracking_contacts_shaped_force 函数

- **作用**：根据期望接触力跟踪奖励。
- **运行方式**：
  1. 计算实际接触力与期望接触力之间的差异。
  2. 使用负指数函数对差异进行处理，生成奖励。
- **为什么这样运行**：确保脚部与地面的接触力在合理范围内，避免过大或过小的接触力。

60. _reward_tracking_contacts_shaped_vel 函数

- **作用**：根据期望脚部速度跟踪奖励。
- **运行方式**：
  1. 计算实际脚部速度与期望速度之间的差异。
  2. 使用负指数函数对差异进行处理，生成奖励。
- **为什么这样运行**：确保脚部速度与期望值一致，鼓励更自然的步态。

61. _reward_feet_height 函数

- **作用**：跟踪脚部高度的奖励函数。
- **运行方式**：
  1. 计算实际脚部高度与目标高度之间的差异。
  2. 对差异进行裁剪和处理，生成奖励。
- **为什么这样运行**：确保脚部高度在合理范围内，鼓励自然的步态。

62. _reward_feet_air_time 函数

- **作用**：奖励脚部在空中的时间。
- **运行方式**：
  1. 计算每只脚的空中时间。
  2. 对空中时间进行处理，生成奖励。
- **为什么这样运行**：鼓励机器人采取更长的步伐，以提高行走效率。

63. _reward_feet_contact_forces 函数

- **作用**：惩罚脚部接触力过大的情况。
- **运行方式**：
  1. 计算脚部接触力，并对超过阈值的部分进行惩罚。
- **为什么这样运行**：保护机器人脚部，避免因过大的接触力而损坏。

64. _reward_orientation 函数

- **作用**：惩罚机器人基座姿态的偏离。
- **运行方式**：
  1. 计算基座姿态的重力投影在XY平面的平方和。
  2. 返回该值作为惩罚。
- **为什么这样运行**：确保机器人在运动过程中保持稳定的姿态，避免倾斜或失衡。

65. _reward_roll 函数

- **作用**：惩罚机器人基座绕X轴的滚转角度偏差。
- **运行方式**：
  1. 计算滚转角度的绝对值。
  2. 返回该值作为惩罚。
- **为什么这样运行**：通过控制滚转角度，保持机器人在运动过程中的平稳性。

66. _reward_base_height 函数

- **作用**：惩罚机器人基座高度偏离目标高度。
- **运行方式**：
  1. 计算基座高度与目标高度的差异。
  2. 返回差异的绝对值作为惩罚。
- **为什么这样运行**：确保机器人在不同地形或任务中的稳定性，防止基座高度过高或过低。

67. _reward_orientation_walking 函数

- **作用**：在行走过程中惩罚基座姿态的偏离。
- **运行方式**：
  1. 调用 `_reward_orientation` 函数计算姿态偏差。
  2. 如果机器人处于行走状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：确保机器人在行走过程中保持稳定的姿态，防止倾斜或失衡。

68. _reward_orientation_standing 函数

- **作用**：在站立过程中惩罚基座姿态的偏离。
- **运行方式**：
  1. 调用 `_reward_orientation` 函数计算姿态偏差。
  2. 如果机器人处于站立状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：类似于行走状态下的惩罚，但专注于机器人站立时的姿态控制。

69. _reward_torques_walking 函数

- **作用**：在行走过程中惩罚扭矩的使用。
- **运行方式**：
  1. 调用 `_reward_torques` 函数计算扭矩惩罚。
  2. 如果机器人处于行走状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：确保机器人在行走过程中尽量减少能量消耗，提高行走效率。

70. _reward_torques_standing 函数

- **作用**：在站立过程中惩罚扭矩的使用。
- **运行方式**：
  1. 调用 `_reward_torques` 函数计算扭矩惩罚。
  2. 如果机器人处于站立状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：类似于行走状态下的惩罚，但专注于机器人站立时的能量消耗。

71. _reward_energy_square_walking 函数

- **作用**：在行走过程中惩罚能量的平方和。
- **运行方式**：
  1. 调用 `_reward_energy_square` 函数计算能量惩罚。
  2. 如果机器人处于行走状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：减少行走过程中的能量浪费，提高效率。

72. _reward_energy_square_standing 函数

- **作用**：在站立过程中惩罚能量的平方和。
- **运行方式**：
  1. 调用 `_reward_energy_square` 函数计算能量惩罚。
  2. 如果机器人处于站立状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：类似于行走状态下的惩罚，但专注于机器人站立时的能量消耗。

73. _reward_base_height_walking 函数

- **作用**：在行走过程中惩罚基座高度的偏离。
- **运行方式**：
  1. 调用 `_reward_base_height` 函数计算基座高度偏差。
  2. 如果机器人处于行走状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：确保机器人在行走过程中保持稳定的基座高度。

74. _reward_base_height_standing 函数

- **作用**：在站立过程中惩罚基座高度的偏离。
- **运行方式**：
  1. 调用 `_reward_base_height` 函数计算基座高度偏差。
  2. 如果机器人处于站立状态，则应用该惩罚，否则忽略。
- **为什么这样运行**：类似于行走状态下的惩罚，但专注于机器人站立时的基座稳定性。

75. _reward_dof_default_pos 函数

- **作用**：惩罚关节位置偏离默认位置的情况。
- **运行方式**：
  1. 计算关节位置与默认位置的误差。
  2. 使用高斯函数对误差进行处理，生成奖励。
- **为什么这样运行**：鼓励关节回到默认位置，避免偏差导致的损坏或不稳定。

76. _reward_dof_error 函数

- **作用**：惩罚关节位置误差。
- **运行方式**：
  1. 计算关节位置与目标位置的平方误差。
  2. 返回误差作为惩罚。
- **为什么这样运行**：减少关节位置的偏差，确保准确执行动作。

77. _reward_tracking_lin_vel_max 函数

- **作用**：跟踪机器人最大线速度的奖励函数。
- **运行方式**：
  1. 计算命令速度与实际速度的最大值。
  2. 对零命令的情况进行特殊处理，返回奖励。
- **为什么这样运行**：确保机器人能够以最大效率跟随线速度命令。

78. _reward_penalty_lin_vel_y 函数

- **作用**：惩罚机器人在Y轴方向的线速度偏差。
- **运行方式**：
  1. 计算Y轴速度的绝对值。
  2. 对特定条件下的情况不应用惩罚。
- **为什么这样运行**：控制机器人在Y轴方向的运动，避免过度偏移。

79. _reward_tracking_contacts_shaped_force 函数

- **作用**：根据期望接触力跟踪奖励。
- **运行方式**：
  1. 计算实际接触力与期望接触力之间的差异。
  2. 使用负指数函数对差异进行处理，生成奖励。
- **为什么这样运行**：确保脚部与地面的接触力在合理范围内，避免过大或过小的接触力。

80. _reward_tracking_contacts_shaped_vel 函数

- **作用**：根据期望脚部速度跟踪奖励。
- **运行方式**：
  1. 计算实际脚部速度与期望速度之间的差异。
  2. 使用负指数函数对差异进行处理，生成奖励。
- **为什么这样运行**：确保脚部速度与期望值一致，鼓励更自然的步态。

81. _reward_feet_height 函数

- **作用**：跟踪脚部高度的奖励函数。
- **运行方式**：
  1. 计算实际脚部高度与目标高度之间的差异。
  2. 对差异进行裁剪和处理，生成奖励。
- **为什么这样运行**：确保脚部高度在合理范围内，鼓励自然的步态。

82. _reward_feet_air_time 函数

- **作用**：奖励脚部在空中的时间。
- **运行方式**：
  1. 计算每只脚的空中时间。
  2. 对空中时间进行处理，生成奖励。
- **为什么这样运行**：鼓励机器人采取更长的步伐，以提高行走效率。

83. _reward_feet_contact_forces 函数

- **作用**：惩罚脚部接触力过大的情况。
- **运行方式**：
  1. 计算脚部接触力，并对超过阈值的部分进行惩罚。
- **为什么这样运行**：保护机器人脚部，避免因过大的接触力而损坏。

以上是 ManipLoco_rewards 类中的所有函数的详细分析，涵盖了每个函数的用途、运行方式以及其存在的原因。这些奖励函数共同作用，以引导机器人在仿真环境中实现特定的行为目标，如跟踪路径、节约能量、保持平衡等。

runner.learn

函数 learn 是 OnPolicyRunner 类中用于执行强化学习训练的核心函数。这个函数实现了强化学习的训练循环，包括采样动作、与环境交互、计算损失、更新策略网络，以及记录和保存训练过程中的数据。

函数的总体结构

learn 函数主要包含以下几个步骤：

1. 初始化部分：包括初始化损失、奖励等指标，设定初始的观测数据，并将策略网络设置为训练模式。
2. 训练循环：这个循环按照预定的学习迭代次数（num_learning_iterations）进行迭代，每次迭代包括环境交互、策略更新、数据记录和模型保存等过程。
3. 学习部分：每次迭代结束后，使用存储的轨迹数据计算策略的损失，并通过反向传播来更新策略网络。
4. 记录与保存：定期记录和保存模型及相关数据。

代码的详细步骤解析

1. 初始化部分：

   • 函数开始时，会初始化一些用于记录损失、奖励等的变量（如 mean_value_loss、mean_surrogate_loss 等）。
   • 如果 init_at_random_ep_len 为 True，则会随机初始化每个环境的初始episode长度。
   • 从环境中获取初始的观察数据 obs （也就是self.obs_buf 6144,744） 和特权观察 privileged_obs （也就是self.privileged_obs_buf None），并将它们转移到指定的设备上（通常是 GPU）。
   • 如果privileged_obs非None，critic_obs就是它否则就是obs

2. 训练循环：

   • 循环从当前的学习迭代数 self.current_learning_iteration 开始，直到总的学习迭代数 tot_iter 45000 结束。
   • 在循环内部：
     • Rollout 阶段：通过与环境交互来采集训练数据。
       • 在每一个时间步（这里共24个step），调用算法的 act 方法来获取动作 actions 6144,18，并与环境进行交互 即 ManipLoco.step(actions)。
       • 交互结果包括新的观察值 obs 6144,744； privileged_obs None、奖励 reward 6144维； arm_rewards 6144维、完成标志 dones 6144维、 infos（一些轮次内的各种率值，times_outs）。这些数据会被存储在算法的存储结构中，供后续的策略更新使用。
     • Learning 阶段：通过算法的 update 方法来更新策略。
       • 在这里，会区分是否进行历史编码的更新（由 dagger_update_freq 控制，其实就是前面的hist_encoding），若满足条件则执行 update_dagger，否则执行普通的 update。也就是计算损失更新梯度的部分，返回一个平均损失
     • 记录与保存：在每次迭代结束时，会记录当前迭代的损失、奖励等信息，并定期保存模型状态。

3. 记录与保存：

   • 函数的最后会更新当前的学习迭代数，并保存模型的状态到指定的路径中。

函数的重要细节

• 多步环境交互：每次 learn 调用会执行多次与环境的交互，通过这种方式收集的轨迹数据将用于后续的策略更新。
• 历史编码更新：在特定条件下（如 dagger_update_freq），会执行特定的 update_dagger 更新，这可能涉及一些与 DAgger（数据集增强）的思想相关的内容。
• 保存模型：模型状态会定期保存，这对大规模强化学习训练非常重要，以防止由于意外终止导致的训练进度丢失。

总结

learn 函数是 OnPolicyRunner 类的核心，负责执行强化学习中的策略训练循环。它通过环境交互、损失计算、策略更新和记录保存等步骤，来逐步优化策略网络，使得代理能够更好地完成任务。这个函数结构严谨，逻辑清晰，涵盖了强化学习训练的方方面面。

ActorCritic(
  # 得到self.transition.actions
  (actor): Actor(输入obs 6144,744
    摘出来前半部分 obs_prop 6144,66
    再对obs后半部分做处理得到 hist 6144,660
    (history_encoder): StateHistoryEncoder( view成6144,10,66后输入     
      (encoder): Sequential( 重塑成61440,66后输入
        (0): Linear(in_features=66, out_features=30, bias=True)
        (1): ELU(alpha=1.0)
      ) 得到projection 61440,30
      (conv_layers): Sequential( 重塑成6144,30,10后输入
        (0): Conv1d(30, 20, kernel_size=(4,), stride=(2,))
        (1): ELU(alpha=1.0)
        (2): Conv1d(20, 10, kernel_size=(2,), stride=(1,))
        (3): ELU(alpha=1.0)
        (4): Flatten(start_dim=1, end_dim=-1)
      )得到output 6144,30
      (linear_output): Sequential(
        (0): Linear(in_features=30, out_features=20, bias=True)
        (1): ELU(alpha=1.0)
      )      
    )    得到latent  6144,20
    拼接到obs_prop后面得到backbone_input 6144,86
    (actor_backbone): Sequential(
      (0): Linear(in_features=86, out_features=128, bias=True)
      (1): ELU(alpha=1.0)
    )    得到backbone_output 6144,128，分别输入给下面两个
    (actor_leg_control_head): Sequential(
      (0): Linear(in_features=128, out_features=128, bias=True)
      (1): ELU(alpha=1.0)
      (2): Linear(in_features=128, out_features=128, bias=True)
      (3): ELU(alpha=1.0)
      (4): Linear(in_features=128, out_features=12, bias=True)
    ) 得到 leg_output 6144,12
    (actor_arm_control_head): Sequential(
      (0): Linear(in_features=128, out_features=128, bias=True)
      (1): ELU(alpha=1.0)
      (2): Linear(in_features=128, out_features=128, bias=True)
      (3): ELU(alpha=1.0)
      (4): Linear(in_features=128, out_features=6, bias=True)
    )    得到 arm_output 6144,6
    先腿后臂拼接得到 mean 6144,18
    从而由mean和self.std构成一个正态分布(torch.distributions.normal.Normal)
    然后从这个分布中取一个样 就是self.transition.actions 6144,18
    


  )
  
  
  # 得到self.transition.value
  (critic): Critic( 输入critic_obs 6144,744
    取了后半一部分 prop_and_priv 6144,84 输入给下面
    (critic_backbone): Sequential(
      (0): Linear(in_features=84, out_features=128, bias=True)
      (1): ELU(alpha=1.0)
    ) 得到backbone_output 6144,128 分别输入给下面两个
    (critic_leg_control_head): Sequential(
      (0): Linear(in_features=128, out_features=128, bias=True)
      (1): ELU(alpha=1.0)
      (2): Linear(in_features=128, out_features=128, bias=True)
      (3): ELU(alpha=1.0)
      (4): Linear(in_features=128, out_features=1, bias=True)
    ) 得到 leg_output 6144,1
    (critic_arm_control_head): Sequential(
      (0): Linear(in_features=128, out_features=128, bias=True)
      (1): ELU(alpha=1.0)
      (2): Linear(in_features=128, out_features=128, bias=True)
      (3): ELU(alpha=1.0)
      (4): Linear(in_features=128, out_features=1, bias=True)
    ) 得到 arm_output 6144,1
  )先腿后臂拼接得到 value 6144,2
)

get_actions_log_prob 得到self.transition.actions_log_prob

作用是计算给定动作的对数概率。对数概率是深度强化学习中经常使用的一种技术，用于评估策略选择某一动作的概率（以对数形式表示），从而在策略梯度算法（如PPO）中进行优化。

1. 计算对数概率 (log_prob):

   • self.distribution.log_prob(actions) 计算 actions 在 self.distribution 定义的分布下的对数概率。log_prob 的形状与 actions 相同，均为 (6144, 18)，每个值表示某一动作在该分布下的对数概率。

2. 腿部和手臂动作对数概率求和:

   • leg_log_prob_sum: 对前 self.num_leg_actions（假设为腿部动作）进行对数概率的求和，结果为 (6144, 1) 的张量。
   • arm_log_prob_sum: 对后 self.num_leg_actions 之后的动作（假设为手臂动作）进行对数概率的求和，结果也是 (6144, 1) 的张量。

3. 拼接对数概率:

   • 最后，将腿部和手臂的对数概率求和值拼接起来，形成一个 (6144, 2) 的张量，其中第一列是腿部动作的对数概率总和，第二列是手臂动作的对数概率总和。

函数最终返回的张量 (6144, 2) 包含了每个环境中的动作序列在策略分布下的对数概率总和。这两个值（腿部和手臂的对数概率总和）对于后续的损失计算和策略更新非常重要。通过这些对数概率，算法可以计算策略的梯度，从而指导模型更新，以优化策略。

process_env_step

函数的主要作用是处理环境中每一步的输出（如奖励、终止状态等），并将这些信息存储起来以供后续的学习步骤使用。具体分析如下：

1. 存储奖励和终止状态:

   • self.transition.rewards = torch.stack([rewards.clone(), arm_rewards.clone()], dim=-1)：
     • 将 rewards 和 arm_rewards 复制并堆叠在一起，形成一个 (6144, 2) 的张量，表示每个环境的奖励和手臂奖励。
   • self.transition.dones = dones：
     • 记录当前步的 dones 信息，即环境是否结束。

2. 超时奖励的处理:

   • if 'time_outs' in infos:：
     • 如果 infos 包含 time_outs，表示有些环境由于超时而结束。
   • self.transition.rewards += self.gamma * torch.squeeze(self.transition.values * infos['time_outs'].unsqueeze(1).to(self.device), 1)：
     • 为了防止在超时情况下丢失未来的价值，函数将通过 gamma（折扣因子）对当前的价值乘以 time_outs，并加到当前奖励上。这是为了进行引导，使得在超时后，策略仍然能考虑未来的奖励。

3. 处理手臂相关信息:

   • if 'target_arm_torques' in infos:：
     • 如果 infos 包含 target_arm_torques，说明这一步需要监督手臂的力矩信息。
   • self.transition.target_arm_torques = infos['target_arm_torques'].detach()：
     • 记录目标手臂力矩，并确保这些张量不会在反向传播中被修改。
   • self.transition.current_arm_dof_pos = infos['current_arm_dof_pos'].detach()：
     • 记录当前手臂的关节位置。
   • self.transition.current_arm_dof_vel = infos['current_arm_dof_vel'].detach()：
     • 记录当前手臂的关节速度。
   • self.storage.add_transitions(self.transition, torque_supervision=True)：
     • 将上述信息保存到 storage 中，用于后续的学习步骤，并指明这一步涉及力矩监督。

4. 不涉及手臂力矩监督的处理:

   • else: self.storage.add_transitions(self.transition, torque_supervision=False)：
     • 如果 infos 中没有 target_arm_torques，则直接将 transition 存储，不进行力矩监督。

5. 清除当前过渡并重置:

   • self.transition.clear()：
     • 清空 transition 对象，为下一步的记录做好准备。
   • self.actor_critic.reset(dones)：
     • 重置 actor_critic 的状态，对于那些已经结束的环境（即 dones 为 True 的情况）。

该函数的核心作用是将环境的每一步输出组织成一个 transition，并根据是否涉及手臂力矩监督将这些信息存储下来。同时，它处理了超时的情况，通过附加奖励的方式来进行引导，防止在超时情况下的策略过早忽略未来奖励。

这个 compute_returns 函数的作用是计算回报（returns）和优势（advantages），这是在策略梯度算法（例如PPO）中用于更新策略的关键步骤。下面是对该函数的详细分析：

compute_returns

• 输入: last_critic_obs 是最后一个观测值，用于计算最后一个时间步的价值函数。
• 输出: 无显式输出，计算的回报和优势会被存储在 self.storage 中。
• 作用:
  • 调用 actor_critic 模型的 evaluate （也就是critic那个模块） 方法，计算出最后一个时间步的状态价值 last_values。
  • 然后调用 self.storage.compute_returns，通过倒序遍历来计算每个时间步的回报（returns）和优势（advantages）。

compute_returns 函数在 Storage 类中的作用

1. 初始化变量:

   • advantage = 0:
     • 初始化优势为0，用于后续的递归计算。

2. 倒序遍历时间步（步骤）:

   • 使用 for step in reversed(range(self.num_transitions_per_env)):
     • 从最后一个时间步开始倒序遍历所有时间步。

3. 计算当前步的价值:

   • next_values = last_values:
     • 如果当前是最后一个时间步（也就是当前循环的第一步），那么下一步的价值next_values等于最后一个时间步的价值。
   • next_values = self.values[step + 1]:
     • 如果不是最后一个时间步，那么下一步的价值等于下一个时间步的价值。

4. 计算非终止标志:

   • next_is_not_terminal = 1.0 - self.dones[step].float():
     • 如果当前步没有终止（即不是最后一步），next_is_not_terminal 等于 1，否则等于 0。

5. 计算 TD-error (Temporal Difference Error):

   • delta = self.rewards[step] + next_is_not_terminal * gamma * next_values - self.values[step]:
     • 这是优势估计中关键的一步，用于衡量当前时间步的奖励和下一个时间步的价值与当前时间步的价值的差异。6144,2

6. 计算优势:

   • advantage = delta + next_is_not_terminal * gamma * lam * advantage:
     • 这里使用了广义优势估计（GAE），通过递归的方式计算当前时间步的优势。6144,2

7. 计算回报（Returns）:

   • self.returns[step] = advantage + self.values[step]:
     • 回报是优势加上当前时间步的价值函数估计值。
     • self.returns 24,6144,2

8. 计算和标准化优势:

   • self.advantages = self.returns - self.values:
     • 优势等于回报减去价值函数估计值。
   • self.advantages = (self.advantages - self.advantages.mean()) / (self.advantages.std() + 1e-8):
     • 对计算得到的优势进行标准化，减去均值并除以标准差。这是为了让优势的分布具有更好的数值稳定性，有助于优化过程。

compute_returns 函数的核心作用是通过 TD-error 结合广义优势估计（GAE）来计算回报（returns）和优势（advantages），这些量最终会被用于策略更新，从而使得策略能够更有效地最大化累计奖励。

update_dagger

作用是使用 DAgger (Dataset Aggregation) 方法对模型进行更新，尤其是更新历史编码器模块以适应当前环境的变化。下面是对该函数的详细分析：

1. 初始化变量:

   • mean_hist_latent_loss = 0:
     • 用于累加每次 mini-batch 更新中计算的历史潜在损失，最终计算平均值。

2. 生成 mini-batch:

   • 根据模型是否为递归模型，选择不同的 mini-batch 生成器：
     • self.storage.reccurent_mini_batch_generator 用于递归模型。
     • self.storage.mini_batch_generator 用于非递归模型。
   • 生成器将生成多个 mini-batch，每个 mini-batch 包含模型训练所需的各种信息，包括观测值、动作、目标值等。

3. 计算和更新历史潜在表示:

   • 对于每个 mini-batch，首先计算出历史潜在表示：
     • 先把obs_batch 36864,744 经过一次act过一遍分布
     • priv_latent_batch = self.actor_critic.actor.infer_priv_latent(obs_batch):
       • 计算出当前批次观测值的私有潜在表示，这个表示是不依赖历史信息的。
     • hist_latent_batch = self.actor_critic.actor.infer_hist_latent(obs_batch):
       • 计算出当前批次观测值的历史潜在表示，这个表示是基于历史信息的。
       • hist_latent_loss = (priv_latent_batch.detach() - hist_latent_batch).norm(p=2, dim=1).mean():
       • 计算历史潜在表示和私有潜在表示之间的差异，即 L2 范数的均值。这个差异被认为是历史潜在表示的损失。

先把obs取后半得到 priv 36864,18      
    (priv_encoder): Sequential(
      (0): Linear(in_features=18, out_features=64, bias=True)
      (1): ELU(alpha=1.0)
      (2): Linear(in_features=64, out_features=20, bias=True)
      (3): ELU(alpha=1.0)
    )
得到 priv_latent_batch 36864,20

4. 更新历史编码器:

   • self.hist_encoder_optimizer.zero_grad():
     • 清零历史编码器优化器中的梯度。
   • hist_latent_loss.backward():
     • 对历史潜在损失执行反向传播，以计算梯度。
   • nn.utils.clip_grad_norm_(self.actor_critic.actor.history_encoder.parameters(), self.max_grad_norm):
     • 对历史编码器的梯度进行裁剪，以防止梯度爆炸。
   • self.hist_encoder_optimizer.step():
     • 根据计算出的梯度更新历史编码器的参数。

5. 累加损失:

   • mean_hist_latent_loss += hist_latent_loss.item():
     • 将当前 mini-batch 的历史潜在损失累加到总损失中。

6. 计算平均损失:

   • mean_hist_latent_loss /= num_updates:
     • 计算平均的历史潜在损失。这里的 num_updates 是总的 mini-batch 数量，即 num_learning_epochs * num_mini_batches。

7. 清理存储:

   • self.storage.clear():
     • 清理存储，用于下一轮训练。

8. 更新计数器:

   • self.update_counter():
     • 更新 DAgger 的计数器，用于记录当前已经进行的更新次数。

9. 返回平均损失:

   • return mean_hist_latent_loss:
     • 返回平均的历史潜在损失，用于后续的日志记录或模型调试。

update_dagger 函数主要通过对历史潜在表示和私有潜在表示之间的差异进行最小化，从而优化模型中的历史编码器模块。这种方法有助于提高模型在不同环境变化下的鲁棒性和适应性。

mini_batch_generator

生成用于训练过程中的小批量数据

1. 计算批次和小批次的大小：

   • batch_size：总样本数（环境数 6144 * 每个环境的转换数 24）。
   • mini_batch_size：每个小批次的样本数。（共 4个batch）

2. 生成随机索引：

   • indices：从 0 到 batch_size - 1 的随机排列索引列表，用于确保随机抽样。

3. 展平数据以便提取小批次：

   • 将所有的数据结构（如 observations、actions、values 等）从形状 (num_envs, num_transitions_per_env) 展平为 (batch_size,)，以便从中提取小批次。

4. 遍历训练周期：

   • for epoch in range(num_epochs)：迭代指定的训练周期数。5

5. 遍历小批次：

   • for i in range(num_mini_batches)：在每个训练周期内迭代指定数量的小批次。4

6. 提取小批次数据：

   • 根据生成的 indices 列表，提取对应的小批次数据：
     • obs_batch：当前小批次的观测数据。
     • critic_observations_batch：当前小批次的评论员观测数据。
     • actions_batch：当前小批次的动作数据。
     • target_values_batch：当前小批次的目标值数据。
     • returns_batch：当前小批次的回报数据。
     • old_actions_log_prob_batch：当前小批次的旧动作的对数概率。
     • advantages_batch：当前小批次的优势数据。
     • old_mu_batch：当前小批次的旧均值数据。
     • old_sigma_batch：当前小批次的旧标准差数据。
     • target_arm_torques_batch：当前小批次的目标手臂扭矩数据。
     • current_arm_dof_pos_batch：当前小批次的当前手臂关节位置数据。
     • current_arm_dof_vel_batch：当前小批次的当前手臂关节速度数据。

7. 生成小批次：

   • 使用 yield 返回当前提取的小批次数据。

整个流程通过不断生成和提取小批次数据，为后续的训练过程提供了随机化且分散的数据样本。

play

1. 创建env
2. env.获取观测值
3. 创建runner

4. 推理

迭代501000次
然后把通过step初始化为0的obs1,744输入给网络（不用再搞分布）输出actions 1,18，再给step