1 简介–Overview1.1 如何使用1.2 学术引用OCS21.3 教程2. 安装–Installation2.1 基础2.2 源代码2.3 依赖2.4 可选依赖2.5 安装2.5.1 Build库3. 开始–Getting Started3.1 从哪开始–Where to start?3.2 最优控制公式3.3 如何定义最优控制3.4 如何设置MPC循环3.5 测试MPC输出4. 优化控制–Optimal Control Modules4.1 成本–Costs4.2 约束–Constrains4.3 动力学–Dynamics4.4 预计算–Pre-cimputation4.5 更改最优控制问题的参数4.6 参考管理器接口4.7 求解器同步模块5. 机器人示例–Robotic Examples5.1 Double Integrator5.2 Cartpole5.3 Ballbot5.4 Quadrotor5.5 Mobile Manipulator :star::star::star:5.5.1 Mabi-Mobile5.5.2 Kinova Jaco25.5.3 Franka Panda5.5.4 Willow Garage PR25.5.5 Clearpath Ridgeback with UR-55.6 Legged Robot6. 从URDF到OCP–From URDF to OCP6.1 质心模型 – Centroidal Model6.2 运动学 – Kinematics6.3 自碰撞检测 – Self Collision Avoidance7. 求解器性能分析–How to Profile the Solvers7.1 测试条件7.2 使用内置定时器7.3 Linux 性能工具7.3.1 概述7.3.2 安装7.3.3 用法7.3.4 从 perf.data 生成火焰图：7.3.5 参考7.4 Valgrind7.5 Cachegrind：缓存和分支预测分析器8 基于神经网络的MPC MPC-Net8.1 Robots8.2 设置8.3 训练8.4 部署8.5 设置新机器人

1 简介–Overview

OCS2 是为状态切换系统最佳控制（Optimal Control for Switched Systems ，OCS2） 量身定制的C++工具箱。该工具箱提供了以下算法的有效实现：
  • SLQ：连续时域约束的DDP。（Continuous-time domain constrained DDP.）
  • iLQR：离散时间域约束DDP。（Discrete-time domain constrained DDP.）
  • SQP：基于HPIPM的多重拍摄算法。（Multiple-shooting algorithm based on HPIPM.）
  • SLP：基于PIPG的顺序线性规划。（Sequential Linear Programming based on PIPG.）
  • IPM：基于非线性内点法的多次射击重算法。（Multiple-shooting algorithm based on nonlinear interior point method.）
OCS2 通过增强拉格朗日或松弛屏障方法处理一般路径约束。为了促进 OCS2 在机器人任务中的应用，它为用户提供了额外的工具来根据 URDF 模型设置系统动力学（例如运动学或动态模型）和成本/约束（例如自碰撞避免和末端执行器跟踪）。该库还提供自动微分工具来计算系统动力学、约束和成本的导数。为了便于在机器人平台上部署，OCS2 提供了 ROS 接口工具。该工具箱的高效且数值稳定的实现及其用户友好的界面为其在板载计算能力有限的众多机器人应用中使用铺平了道路。

1.1 如何使用

OCS2 可以轻松地安装在 Ubuntu 上。要开始使用控制工具箱，请参阅开始和入门页面。

1.2 学术引用OCS2

1.3 教程

OCS2 工具箱教程、Farbod Farshidian、MPC Workshop、RSS 2021（链接）。
腿部运动和操纵的实时最优控制，Marco Hutter，MPC Workshop，RSS 2021（链接）。

2. 安装–Installation

2.1 基础

OCS2 库是用 C++11 编写的。它在 Ubuntu 20.04 下进行了测试，库版本如软件包源中提供的那样。

2.2 源代码

源代码托管在 GitHub 上：leggedrobotics/ocs2。

2.3 依赖

  • 支持 C++11 的 C++ 编译器
  • Eigen (v3.3)
  • Boost C++ (v1.71)
  • GLPK sudo apt install libglpk-dev
  • catkin sudo apt-get install catkin
  • pybind11_catkin（ROS包） sudo apt install ros-noetic-pybind11-catkin
  • catkin-pkg（Python3 的包） sudo apt install python3-catkin-tools
  • Doxygen（用于文档编译） sudo apt install doxygen doxygen-latex

2.4 可选依赖

  • 对于刚性多体动力学库和自碰撞支持，将 Pinocchio 和 HPP-FCL 克隆到您的工作空间中
    • 对于 OCS2 单元测试和机器人示例中使用的各种机器人资料
      • rqt_multiplot 包可用于可视化求解器的性能和其他优化输出
        • RaiSim 模拟器可以用作部署的提供者。相应的 ocs2_raisim 包有额外的要求，RaiSim 软件包，需要通过将其克隆到工作区来从源安装：
          • 安装时，请按照 RaiSim 网页的说明进行操作。或者，为了使catkin的安装更容易找到并且将来易于卸载，请考虑使用CheckInstall，它会将RaiSim安装为debian软件包。为此，替换原来的命令：
            • 以及
              • 这会将 RaiSim 安装在默认位置 /usr/local/lib ，catkin 也会自动检测到该位置。当 checkinstall 要求时，将包名称更改为适当的名称（例如“raisim”），以便稍后查找以进行包管理，例如卸载（ dpkg -r <package_name> ）。
              • 对于可视化，请使用 RaiSim Unity，其中 raisimLib/raisimUnity 目录中提供了预构建的可执行文件。例如，它可用于调试目的，以查看 OCS2 和 RaiSim 之间的转换是否正确。
            • 网格地图catkin包，可以通过 sudo apt install ros-noetic-grid-map-msgs 安装。
            • ONNX Runtime 是推理和训练加速器。在这里，它用于在 C++ 代码中部署学习的 MPC-Net 策略。要在本地安装它，请执行以下操作：
                • 我们提供自定义 cmake 配置和版本文件来启用 find_package(onnxruntime) ，而无需修改 LIBRARY_PATH 和 LD_LIBRARY_PATH 。请注意，上面的最后一个命令假设您将 OCS2 克隆到用户主目录中的文件夹 git 中。
            • 训练 MPC-Net 策略时建议使用虚拟环境：
                • 创建一个环境并授予其访问系统站点包的权限：
                激活环境并安装依赖包：
                较新的显卡可能需要 CUDA 功能，但标准 PyTorch 安装目前不支持该功能。在这种情况下，请检查 PyTorch Start Locally 以获得兼容版本，然后运行：

            2.5 安装

            2.5.1 Build库

            1. 创建一个新的catkin工作区：
              1. 克隆 OCS2 库：
                1. 构建并运行单元测试：
                    2. PS: 也可以构建单独的包，例如构建示例文件夹中的ocs2_mobile_manipulator_ros

                3. 开始–Getting Started

                我们在此简要说明为您的特定问题应用 OCS2 所需采取的步骤。根据安装页面中的描述，我们已经假设您已经成功安装了该库。

                3.1 从哪开始–Where to start?

                第一步，也许也是最重要的一步，是决定您要使用什么模型复杂性来解决手头的问题。例如，运动学模型是否足以满足您计划使用 MPC 解决的任务？一些常见的模型是：
                • 运动学模型（Kinematic model）：该模型通常用于固定基座或移动基座的机械臂。该问题的目标是：末端执行器跟踪、自碰撞避免和无碰撞导航。有关示例，请参阅移动操纵器示例。
                • 动力学模型（Dynamic model）：这是最完整的刚体模型，同时考虑了系统的运动学和动力学。有关示例，请参阅 Ballbot 示例。
                • 质心模型（Centroidal model）：这是一种==常用于腿式机器人的模型==，因为它具有两个世界中最好的模型。首先它考虑系统的完整运动学。其次，它只考虑受外力影响的部分动力学，而不是计算量大的完整动力学。有关示例，请参阅腿式机器人示例。

                3.2 最优控制公式

                OCS2 专为解决切换系统的最优控制问题而定制。切换系统由有限数量的动态子系统组成，这些子系统受到离散事件的影响，导致这些子系统之间发生转换。切换系统模型在许多实际应用中都会遇到，例如具有不同齿轮的汽车和机车、DC-DC 转换器、制造过程、生物系统和行走机器人。这种系统的最优控制可以表述为：
                notion image
                式中，ts切换时间，tI为终止时间，对于每种模式，非线性成本函数由预跳跃成本和中间成本组成。虽然乍一看这个公式似乎有点吓人，但它实际上有一个简单的解释，并为定义 MPC 问题带来了最大的灵活性。人们可以将此问题想象为一系列最优控制子问题，这些子问题通过系统跳转图相互连接。每个子问题甚至可以具有不同的状态和输入维度，只要在它们之间正确定义跳转图即可。如前所述，该公式中的切换是基于时间触发的事件。这意味着您需要定义模式的顺序以及发生切换的事件时间。
                对于许多机器人平台，例如四旋翼飞行器、ballbot 和 cartpole，系统只有单一模式（即，它们是单域系统）。在这些情况下，上述公式简化为：
                notion image
                这是一个常规最优控制问题的形式。

                3.3 如何定义最优控制

                为了定义最优控制，您至少需要定义系统动力学和相对输入的具有正定 Hessian 矩阵的成本函数，以及一个相对状态的正半定 Hessian 矩阵。幸运的是，OCS2 提供了一个紧密匹配的接口来定义最优控制问题，称为 Optimal Control Problem（请参阅本页）。

                3.4 如何设置MPC循环

                到目前为止，您已经创建了一个最优控制问题。要设置 MPC，您需要在每个控制刻度处使用最新的状态测量值重复解决此问题。虽然对于简单的系统来说，实时解决这个问题是可能的，但对于许多板载计算能力有限和控制频率环路较高的机器人平台来说，这是不可能的。为此，您需要满足以下条件：
                1. 使用最新的状态测量尽可能快地运行 MPC。
                1. 使用最新的 MPC 输出，在读取其输出时无需担心任何赛车问题。为此，您需要一些同步机制来满足这些要求。 OCS2通过引入MPC接口和MRT（Model Reference Tracking, MRT）接口的概念来提供此类功能。
                MPC接口
                MPC 接口负责使用最新的测量结果安全地更新求解器。因此，用户可以安全地为求解器设置最新状态并推进它。如果解算器尚未从上一次调用中终止，则状态将被缓冲，直到解算器准备好为止，缓冲区大小为 1，因此解算器始终会获得最新状态。
                MRT接口
                MRT 接口负责安全访问求解器的结果。它提供了两种输出视图：基于时间的视图和基于状态的视图。在基于时间的方法中，MRT 仅根据优化状态输入轨迹的线性插值输出查询时间的优化状态输入对。基于状态的技术使用给定时间和状态的反馈策略来评估最优输入。请注意，应在求解器设置中激活反馈策略选项（假设求解器支持反馈策略）。
                ROS和非ROS版本
                MPC 和 MRT 接口协同工作，您需要同时在机器人上部署 MPC。根据您在单台计算机或不同计算机上运行 MPC 和 MRT，您应该使用 MpcMrtInterface ，或 MpcRosInterface 和 MrtRosInterface 对。顾名思义，后者使用 ROS 在 MPC 和 MRT 节点之间进行通信。

                3.5 测试MPC输出

                最后阶段是调整成本和其他算法超参数。为了将规划问题与跟踪控制器（将 MPC 输出转换为机器人命令输入（例如扭矩、所需的关节角度和速度））分开，OCS2 配备了虚拟模拟器。最简单的形式是，MRT 虚拟模拟器仅插入优化的状态输入并在 rviz 中将其可视化。但是，如果您将 Rollout 实例设置为虚拟模拟器，它将使用它来模拟 MPC 策略。在这种情况下，如果您采用与最优控制问题中使用的相同的动力学，您将使用用于规划的精确模型来模拟 MPC 输出。更先进的模拟器（例如 RaiSim）也可以用作推出实例。

                4. 优化控制–Optimal Control Modules

                在此部分，我们为您提供了一些有关如何定义 MPC 问题的提示。
                在 OCS2 中，Optimal Control Problem 结构定义了==优化问题的主要组成部分，即动态、成本和约束==。除此之外，您可能希望向 MPC 提供一些参考轨迹、预定义模式时间表（对于切换系统）以及其他外部信息，例如模型参数的更新（例如，在自适应控制设置中）、环境地图（例如，用于避免碰撞的 SDF 地图）等。所有这些都将在 Reference Manager 和 Solver Synchronized Module 的帮助下成为可能。

                4.1 成本–Costs

                如前所述，成本函数是为三个不同的时间实例定义的：中间成本、预跳转成本和最终成本。
                中间成本项可以是时间、状态和输入 (OptimalControlProblem::costPtr) 或时间和状态 (OptimalControlProblem::stateCostPtr) 的函数。
                预跳转和最终成本项应该只是时间和状态的函数。
                成本项应该继承自 StateCost 或 StateInputCost 类。派生类应定义成本值及其二次近似值。对于复杂的函数，您可以使用这些类的自动微分版本，即 StateCostCppAd 或 StateInputCostCppAd，其中用户只需要提供成本值。有关这些类的实现细节，请参阅“ocs2_core/cost”，有关简单示例请参考 QuadraticStateCost 和 QuadraticStateInputCost。
                注意：我们所有的最优控制求解器都假设相对于输入的中间成本项的 Hession 之和在每个中间时间都是正定的。此外，除了 PISOC 之外，求解器要求所有项相对状态的 Hessian 矩阵之和在每个时间实例（中间、跳跃前和最终）也是半正定的。请注意，虽然在线搜索策略和信任区域全球化策略中使用粗麻布校正在某种程度上可以处理非定性，但如果保证这种正定性，求解器通常会更可靠地工作。避免非定性的一种方法是对 ∥f(xu)∥2形式的成本函数使用高斯-牛顿近似技术，其中 f(xu)的线性近似将用于形成正定 Hessian 矩阵。有关更多详细信息，请参阅 StateInputCostGaussNewtonAd。

                4.2 约束–Constrains

                与成本类似，也为三个不同的时间实例定义了约束：中间时间、预跳转时间和最终时间，其中中间时间约束是时间、状态、输入或时间、状态和预跳转时间的函数，而最终时间约束仅是时间和状态的函数。
                约束项应该继承自 StateConstraint 或 StateInputConstraint 类。派生类应根据约束的顺序 (ConstraintOrder) 定义约束值及其线性或二次近似值。对于复杂的函数，您可以使用这些类的自动微分版本，即 StateConstraintCppAd 或 StateInputConstraintCppAd，其中您只需要提供约束值。有关这些类的实现细节，请参阅“ocs2_core/constraint”，有关简单示例，请检查 LinearStateConstraint 和 LinearStateInputConstraint。
                为了处理 OCS2 中的约束，您可以使用==硬约束==方法或==软约束==方法。
                ==软约束==由 OptimalControlProblem 单独收集。软约束处理基于惩罚方法，其中约束用 用户定义的惩罚函数包装（有关这些惩罚函数的列表，请参阅“ocs2_core/soft_constraint/penalties”）。要根据约束项创建软约束，您可以使用 StateSoftConstraint 和 StateInputSoftConstraint 类。这些类采用约束项和选择的惩罚函数的实例，并创建软约束收集器可以收集的成本项。与约束一起设置惩罚函数的可能性提供了为每个约束（不同类型和/或不同的超参数）使用不同惩罚函数的灵活性。这使得约束违反的调整变得更容易。
                ==硬约束==（称为约束）根据其类型通过不同的技术进行更高精度的处理。状态输入等式约束通过投影方法处理。仅状态相等和不等式通过松弛屏障方法或增强拉格朗日技术处理（此功能暂时禁用，将在下一版本中添加）。
                注意：由于状态输入等式约束是通过投影方法处理的，因此 OCS2 假定约束的 Jacobin 关系相对输入是满秩的。如果不能保证这个条件，则应使用软约束技术。

                4.3 动力学–Dynamics

                动力学由其流图、跳跃图及其一阶近似值定义（请参阅 SystemDynamicsBase）。对于常规系统，跳跃映射是恒等映射，但对于切换系统，该映射可以是状态的非线性函数。对于复杂的函数，您可以使用动力学 SystemDynamicsBaseAD 的自动微分版本。

                4.4 预计算–Pre-cimputation

                OCS2 是缓存友好的，这意味着您可以在成本、约束、系统动态及其近似值之间共享计算。为了实现这一点，OCS2 使用预计算。在评估成本、约束和系统动态之前，OCS2 求解器会调用 PreComputation::request（切换时的 requestPreJump 或最终时间的 requestFinal），并带有指示下一步将执行哪些操作的请求消息。因此，您可以根据指示请求集的输入参数来实现 request() 方法。
                注意：作为一般规则，您应该避免使用缓存，并且仅在以后实现缓存版本以获得性能。有关示例，请参阅“ocs2_robotic_examples/ocs2_mobile_manipulator”。

                4.5 更改最优控制问题的参数

                一旦您定义了 OptimalControlProblem 并为其设置为求解器，求解器就会在内部创建它的副本（实际上是许多副本）。因此，如果您决定更改最优控制问题中的任何参数（例如参考轨迹或模型参数），则无法通过简单地修改您有权访问的最优控制问题的参数来实现此目的。而且，无论这个技术点如何，您都应该避免随时随意更改这些参数。原因是当您更改参数时，您的 MPC 解算器可能处于迭代中间，这将导致解算器中出现未定义的行为。为了解决这个问题，OCS2 引入了参考管理器和求解器同步模块的概念。一般来说，这些是同步概念，并确保参数更新在正确的时间发生（在 MPC 的每次迭代之前和/或之后）。换句话说，它们将参数更新与 MPC 迭代同步。
                要访问最优控制组件中更新的参数/信息（例如成本、约束、动态等），您需要执行以下步骤： (1) 创建同步模块的共享指针。 (2) 共享此实例的地址以及您的成本、约束或动态。 (3) 通过SolverBase::setReferenceManager、SolverBase::addSynchronizedModule 或SolverBase::setSynchronizedModules 将其设置到求解器。
                注意：整个 MPC 问题中每个同步模块应该只有一个实例。

                4.6 参考管理器接口

                ReferenceManagerInterface 创建一个通用接口，用于定义目标轨迹和模式时间表（仅在切换系统中使用）。 OCS2 的每个求解器在开始新的 MPC 迭代之前都会调用参考管理器的 preSolverRun()。该接口的实现可以参考ReferenceManager类。 ReferenceManager 有两个修饰器类：ReferenceManagerRos，将 ROS 通信添加到 ReferenceManager 和 LoopshapingReferenceManager，将其扩展到循环形状的 OptimalControlProblem。
                由于参考管理器按照 MPC 主循环的顺序运行，出于效率原因，您应该避免在 preSolverRun 中进行复杂的操作。为此，您应该在不同的线程中处理这些参数并将结果保存在缓冲存储器中。然后在preSolverRun中，只需通过地址交换更新活动参数即可。为此，OCS2 提供了一个名为 BufferedValue 的帮助程序类。

                4.7 求解器同步模块

                SolverSynchronizedModules 与 ReferenceManagerInterface 类似，但适用于通用应用程序。它只有两个纯虚方法 preSolverRun 和 postSolverRun，顾名思义，它们在每次 MPC 迭代之前和之后调用。 preSolverRun 方法还可以访问最近更新的 ReferenceManagerInterface。相反，postSolverRun 方法可以访问 MPC 解决方案。
                与ReferenceManagerInterface类似，SolverSynchronizedModules按照MPC的主循环顺序运行。因此，出于效率考虑，您应该避免在 preSolverRun 和 postSolverRun 中进行复杂的操作。为此，您应该在不同的线程中保存/计算这些参数，并将结果保存在缓冲区中。然后在 preSolverRun 或 postSolverRun 中，您可以通过地址交换来更新活动参数。您可以使用 BufferedValue 类来实现此目的。

                5. 机器人示例–Robotic Examples

                OCS2 包括几个机器人示例。我们在这里简要讨论每个示例的主要特征。
                系统
                状态维度
                输入维度
                约束
                缓存
                Double Integrator
                2
                1
                No
                No
                Cartpole
                4
                1
                Yes
                No
                Ballbot
                10
                3
                No
                No
                Quadrotor
                12
                4
                No
                No
                Mobile Manipulator
                6-13
                6-13
                Yes
                Yes/No
                Legged Robot
                24
                24
                Yes
                No
                对于所有这些机器人示例，有两个单独的包：
                • ocs2_<robot>：为库提供特定于机器人的 MPC 实现。
                • ocs2_<robot>_ros: 使用 ROS 包装 MPC 实现来定义 ROS 节点

                5.1 Double Integrator

                Double Integrator示例是我们最简单的问题。它对沿 x 方向移动的一维点质量进行建模。该模型是线性的，成本函数是二次的。通过参考管理器模块将目标点设置为二次成本。
                notion image

                5.2 Cartpole

                推车杆示例是一个经典的控制问题，其中一根杆通过未驱动的关节连接到推车上。汽车沿着无摩擦的轨道行驶。目标是通过沿着轨道加速或减速小车，同时遵守输入限制，从垂直位置开始向上摆动并平衡钟摆。
                notion image

                5.3 Ballbot

                Ballbot 示例是一个 5DoF 系统。该平台是一个扭矩控制的全向机器人，通过三个全向轮在球上保持平衡。该系统具有非线性动力学并表现出非最小相位行为。系统动力学基于 Ballbot 的前向动力学，并且通过自动微分计算流量图的线性近似。任务目标是根据用户命令控制机器人的 XY 位置和偏航。
                notion image

                5.4 Quadrotor

                Quadrotor示例是 6DoF 系统。该平台被建模为浮基刚体动力学，在机器人的法向方向上具有 3D 力矩和 1D 力控制。系统动力学及其导数是由代码生成的。此示例旨在跟踪定义为四旋翼飞行器 3D 位置和偏航的用户命令。
                notion image

                5.5 Mobile Manipulator :star::star::star:

                移动操纵器示例是一个完全运动学问题。该模型由 6DOF 臂以及移动底座的 2D 位置和航向组成。控制输入是手臂的 6 个关节速度以及底座的前进速度和旋转速度。该任务的目标是跟踪 6DoF 末端执行器姿势。关节位置和速度限制包含在最优控制问题的约束中。自碰撞避免是基于URDF模型中的碰撞体和碰撞避免约束来实现的（参考ocs2_self_collision）。
                注意：此示例实现了 MPC 的缓存和非缓存变体，可以通过配置文件中的 usePreComputation 标志进行选择。
                通过解析URDF和任务文件来确定系统模型。目前支持以下系统型号：
                • Default（值：0）：解析URDF得到的默认系统模型。
                • 驱动虚拟轴距（值：1）：向从 URDF 解析的模型中添加虚拟 XY-Yaw 关节，该关节在完整约束（速度控制）下驱动。这是带有 SE(2) 控制的底座的移动机械手模型。
                • Unactuated Dummy float-base（值：2）：将虚拟 XYZ-RPY 关节添加到从 URDF 解析的未驱动模型中。
                • Actuated Dummy float-base（值：3）：将虚拟 XYZ-RPY 关节添加到从 URDF 解析的模型中，该模型是完全驱动的（速度控制）。这是带有 SE(3) 控制的底座的移动机械手模型。
                要尝试不同的模型类型，您可以更改 task.info 文件中的模型信息。
                ==对于几种常见的机器人操纵器，我们提供了使用 OCS2 运行它们的示例。此处提供了生成机器人模型文件 (URDF) 所采取的步骤。==
                对于其中一些示例，我们通过 task.info 文件中的 model_information.removeJoints 属性修复 URDF 中存在的某些关节（例如夹具手指关节或轮关节）。这可以简化构建的Pinocchio模型。仅出于 rviz 上的可视化目的，task.info 中的removeJoints 下的关节由虚拟模拟节点发布在零关节位置。

                5.5.1 Mabi-Mobile

                notion image

                5.5.2 Kinova Jaco2

                notion image

                5.5.3 Franka Panda

                notion image

                5.5.4 Willow Garage PR2

                notion image

                5.5.5 Clearpath Ridgeback with UR-5

                notion image

                5.6 Legged Robot

                有腿机器人的例子是一个切换系统问题。它采用 MPC 方法来控制四足机器人 Anymal 的运动。机器人的步态由用户定义，并且可以在执行过程中通过解算器同步模块（GaitReceiver）进行修改。模式序列和目标轨迹通过参考管理器模块（SwitchedModelReferenceManager）定义。成本函数是二次惩罚，用于跟踪命令的基本位置和偏航，并将机器人的重量平均分配在站立脚上。该问题有几个与模式相关的约束，例如摆动脚的力为零和站立脚的速度为零。摩擦锥强制施加接触力，并且为了避免脚部擦伤，摆动脚沿 z 方向跟踪预定运动。
                系统动力学以两种方式建模，可以从配置文件中选择：
                1. 单刚体动力学 (single rigid body dynamics, SRBD)：该模型假设系统具有恒定的惯性，无论其关节位置如何。它还包括系统的完整运动学 .
                1. 全质心动力学 (full centroidal dynamics , FCD)：该模型使用质心动力学，其中包含机器人四肢的运动。与 SRBD 类似，它考虑机器人的完整运动学。
                notion image

                6. 从URDF到OCP–From URDF to OCP

                阻碍 MPC 在机器人任务中广泛应用的主要挑战之一是建立最优控制问题的负担。 OCS2 提供了几个帮助程序类，用于定义一些常用模型、成本和约束，以缓解此问题。为此，OCS2 连接到多个第三方软件包，例如 RobCoGen、CppADCodeGen、Pinocchio 和 HPP-FCL。我们在这里重点关注我们的 Pinocchio 界面。本页讨论的所有包都可以在元包 ocs2_pinocchio 中找到。

                6.1 质心模型 – Centroidal Model

                多关节浮基系统（例如腿式机器人）可以建模为未驱动的 3D 刚体，其上附有一组完全驱动的肢体。在机器人关节具有足够控制权的温和假设下，独立考虑 MPC 公式中的质心动力学作为简化的模板模型是合理的。该模型的状态空间包括归一化质心动量、基坐标和关节位置。输入空间是所有接触扳手和关节速度的串联。为了捕捉广义坐标变化率对质心动量的影响，该模型使用质心动量矩阵，并引入基础扭曲和关节速度之间的正确映射。有关实现的更多详细信息，请参阅包 ocs2_centroidal_model。

                6.2 运动学 – Kinematics

                OCS2 为基于 Pinocchio 库的 URDF 模型中的任何命名框架提供运动学接口。该接口为命名框架列表提供位置、方向误差和速度的一阶模型。 OCS2 提供两个接口：PinocchioEndEffectorKinematics（基于分析偏差）和 PinocchioEndEffectorKinematicsCppAd（基于自动微分）。当想要依赖OCS2的缓存能力时，通常使用前者；否则，应使用 CppAd 变体。

                6.3 自碰撞检测 – Self Collision Avoidance

                OCS2 库提供了用于定义自碰撞避免约束的辅助类。这些约束可以通过 URDF 模型和用户定义的碰撞体列表方便地定义。该列表应该是 URDF 模型碰撞体的子集。该列表用于避免所有主体之间的碰撞检查，并减少计算开销。碰撞约束计算需要 HPP-FCL 和 Pinocchio 库。有关实现的更多详细信息，请参阅包 ocs2_self_collision

                7. 求解器性能分析–How to Profile the Solvers

                本部分列出了一些分析和基准测试工具以及如何将它们与 OCS2 一起使用。

                7.1 测试条件

                为了提高准确性并使比较公平，请关闭 powersave 和 Turbo boost：
                您可能还想在发布模式下构建并启用架构特定的功能：

                7.2 使用内置定时器

                OCS2 保留求解器计时的内部统计数据。如果启用，求解器将从析构函数打印摘要。您可以在 task.info 文件中启用它们：
                或者您可以直接通过 getBenchmarkingInfo() 方法访问它们。

                7.3 Linux 性能工具

                7.3.1 概述

                • Perf 收集您流程的统计数据
                • perf stat ：CPU性能计数器
                  • instructions 指示
                  • branches, branch-misses 分支、分支未命中
                  • L1, LLC cache loads and misses L1、LLC 缓存加载和未命中
                  • context-switches 上下文切换
                • perf record :
                  • 查看时间花在哪些功能上
                • 注意：perf 不提供精确的测量结果，而仅提供统计近似值！

                7.3.2 安装

                7.3.3 用法

                • 用 DCMAKE_CXX_FLAGS=-fno-omit-frame-pointer 编译
                • 运行目标进程并检索 PID：例如 ps -eo pid,command | grep ocs2_anymal_croc_mpc_node | grep -v grep
                • 用于记录性能数据： sudo perf record -g -p PID
                • 用于收集性能计数器统计信息： sudo perf stat -d -p PID
                • 使用 Ctrl-C 停止录制。数据分别写入 perf.data 或stderr。
                • 使用以下命令分析 perf.data： sudo perf report -g ’graph,0.5,caller -i perf.data
                • 替代方案：rosrun ocs2_benchmark run_perf.py stat timeout=60 output=stat.csv`：
                  • 自动查找并附加到进程
                  • 运行给定的超时时间

                7.3.4 从 perf.data 生成火焰图：

                • git clone https://github.com/brendangregg/FlameGraph.git
                • cd FlameGraph
                • perf script --max-stack=20 -i path/to/perf.data | ./stackcollapse-perf.pl | ./flamegraph.pl > flame.svg
                • 在 Web 浏览器中打开交互式 SVG

                7.3.5 参考

                7.4 Valgrind

                安装
                Massif：堆分析器
                Valgrind Massif 分析定期拍摄的快照的内存使用情况。如果调试符号可用，它允许分析内存使用情况到各个函数和行。
                用法
                • 使用 launch-prefix="valgrind --tool=massif" 启动
                • 检索 ~/.ros/massif.out.PID
                • ms_print massif.out.PID | less -S
                massif-visualizer GUI:
                • sudo apt-get install massif-visualizer
                • massif-visualizer massif.out.PID
                参考：Massif 手册

                7.5 Cachegrind：缓存和分支预测分析器

                用法
                • 使用调试信息进行编译： DCMAKE_BUILD_TYPE=RelWithDebInfo
                • 使用 launch-prefix="valgrind --tool=cachegrind" 启动
                • 检索 ~/.ros/cachegrind.out.PID
                • cg_annotate cachegrind.out.PID | less -S
                参考：Cachegrind手册

                8 基于神经网络的MPC MPC-Net

                MPC-Net 是一种模仿学习方法，它使用 MPC 的解决方案来指导策略搜索。主要思想是通过最小化控制哈密顿量来模拟 MPC，同时通过参数化策略表示相应的控制输入。 MPC-Net 可用于将模型预测控制器克隆到神经网络策略中，其评估速度比 MPC 快得多。因此，在不需要最精确解决方案的计算要求较高的应用程序中，MPC-Net 是 MPC 的有用代理。
                多线程数据生成和策略评估与策略训练异步运行。数据生成和策略评估用C++实现并在CPU上运行，而策略训练用Python实现并在GPU上运行。控制哈密顿量由线性二次近似表示。因此，训练可以在 GPU 上运行，无需回调在 CPU 上运行的 OCS2 C++ 代码来评估哈密顿量，并且可以在 GPU 上利用批处理。

                8.1 Robots

                MPC-Net 已针对以下机器人示例实施：
                ㅤ
                推荐 CPU核心
                推荐显存
                RaiSim
                训练时间
                ballbot
                4
                2GB
                No
                0min 20s
                legged_robot
                12
                8GB
                Yes/No
                7min 40s

                8.2 设置

                确保遵循安装页面。请遵循依赖项的所有说明。关于可选依赖项，请确保遵循 ONNX 运行时和虚拟环境的说明，并可选择设置 RaiSim。
                要构建所有 MPC-Net 包，请构建元包：
                要构建特定于机器人的包，请将 <robot> 替换为机器人名称：

                8.3 训练

                要训练 MPC-Net 策略，请运行：
                要使用 Tensorboard 监控训练进度，请运行：
                如果您使用 RaiSim，则可以使用 RaiSim Unity 可视化数据生成和策略评估部署，其中预构建的可执行文件在 RaiSim 的 raisimUnity 文件夹中提供。例如，在 Linux 上运行：

                8.4 部署

                要部署存储在机器人特定包的 policy 文件夹中的默认策略，请运行：
                要部署存储在机器人特定包的 python/ocs2_<robot>_mpcnet/runs 文件夹中的新策略，请将 <path> 替换为最终策略的绝对文件路径并运行：

                8.5 设置新机器人

                为新机器人设置 MPC-Net 相对容易，因为 ocs2_mpcnet_core 包负责数据生成和策略评估部署，并实现重要的学习组件，例如内存、策略和损失函数。
                本节假设您已经拥有用于机器人特定 MPC 实现的软件包：
                1. ocs2_<robot>：为库提供特定于机器人的 MPC 实现。
                1. ocs2_<robot>_ros：使用 ROS 包装 MPC 实现来定义 ROS 节点
                1. ocs2_<robot>_raisim：（可选）机器人特定 MPC 实现与 RaiSim 之间的接口。
                对于实际的 ocs2_ _mpcnet 包，请遵循现有机器人特定 MPC-Net 包的结构。必须实现的最重要的类/文件是：
                • <Robot>MpcnetDefinition：定义 OCS2 状态变量如何转换为策略观测值。以及政策行动如何转化为 OCS2 控制输入。
                • <Robot>MpcnetInterface：提供C++和Python之间的接口，允许交换数据和策略。
                • <Robot>.yaml：存储配置参数。
                • mpcnet.py：添加机器人特定的方法，例如实现机器人应执行的任务，以进行 MPC-Net 训练。
                • train.py：启动主训练脚本。
                已知问题
                严格的不等式约束可能会导致在对数障碍附近出现非常大的哈密顿量和哈密顿量梯度。这可能会阻碍学习过程，并且策略可能无法学到有用的东西。在这种情况下，请在机器人的 MPC-Net YAML 配置文件中启用梯度裁剪并调整梯度裁剪值。
                参考
                1. Jan Carius, Farbod Farshidian, and Marco Hutter. Mpc-net: a first principles guided policy search. IEEE Robotics and Automation Letters, 5(2):2897–2904, 2020.
                1. Alexander Reske, Jan Carius, Yuntao Ma, Farbod Farshidian, and Marco Hutter. Imitation learning from mpc for quadrupedal multi-gait control. In 2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021.