文件框架 ¶
约 2122 个字 1 行代码 预计阅读时间 8 分钟
项目概述 ¶
ocs2_ros2-ros2 是基于瑞士 ETH Zurich 腿足机器人实验室开发的 OCS2（Optimal Control for Switched Systems） 工具箱，经过重构以兼容 ROS2（Humble/Jazzy）和现代 CMake 体系。整体采用 C++17 标准，使用 ament_cmake 构建系统，支持 DDP/SQP/IPM 等多种最优控制求解器，并提供从简单倒摆到复杂四足机器人的完整应用示例。
顶层目录结构 ¶
F:\ROS\ocs2_ros2-ros2\
├── .clang-tidy # C++ 静态分析配置（代码风格/lint规则）
├── .gitignore # Git 忽略规则
├── .gitmodules # Git 子模块配置（ocs2_robotic_assets, plane_segmentation_ros2）
├── .images/ # README 中引用的演示图片
├── LICENCE.txt # BSD 开源许可证
├── README.md # 项目主文档（安装指南、示例说明）
│
├── core/ # [核心层] OCS2 底层核心库
├── mpc/ # [求解器层] MPC 求解算法库
├── robotics/ # [机器人接口层] ROS2 接口与机器人工具
├── basic examples/ # [基础示例] 六大标准机器人示例
├── advance examples/ # [高级示例] 感知/仿真/神经网络控制
└── submodules/ # [子模块] 第三方/辅助模块
架构层级关系 ¶
┌─────────────────────────────────────────────────────────────────────┐
│ OCS2_ROS2 架构层级关系 │
├─────────────────────────────────────────────────────────────────────┤
│ [高级示例层] advance examples/ │
│ ocs2_perceptive_anymal │ ocs2_raisim │ ocs2_mpcnet │
├─────────────────────────────────────────────────────────────────────┤
│ [基础示例层] basic examples/ │
│ double_integrator │ cartpole │ ballbot │ quadrotor │
│ mobile_manipulator │ legged_robot (各含 _ros ROS2节点包) │
├─────────────────────────────────────────────────────────────────────┤
│ [机器人接口层] robotics/ │
│ ocs2_msgs │ ocs2_ros_interfaces │ ocs2_pinocchio(5子包) │
│ ocs2_robotic_tools │ ocs2_python_interface │
├─────────────────────────────────────────────────────────────────────┤
│ [求解器层] mpc/ │
│ ocs2_mpc(基类) │ ocs2_ddp(DDP/SLQ/ILQR) │ ocs2_sqp(SQP+HPIPM) │
│ ocs2_ipm(内点法) │ ocs2_slp(序列线性) │ ocs2_qp_solver(QP基础) │
├─────────────────────────────────────────────────────────────────────┤
│ [核心层] core/ │
│ ocs2_core(数学基础/动力学/代价/约束/积分/自动微分) │
│ ocs2_oc(OCP问题定义/多重打靶/rollout/求解器基类) │
│ ocs2_thirdparty(CppAD头文件/IIT-RBD) │
├─────────────────────────────────────────────────────────────────────┤
│ [子模块] submodules/ │
│ grid_map_sdf │ ocs2_robotic_assets* │ plane_segmentation_ros2* │
└─────────────────────────────────────────────────────────────────────┘
*号表示 Git 子模块，需手动初始化（git submodule update --init --recursive）
各层详细分析 ¶
core/ — 核心算法层 ¶
职责：提供 OCS2 框架的数学基础、最优控制问题定义及第三方依赖封装。
core/
├── ocs2_core/ # 核心数学与控制基础库（最大最重要的包）
│ ├── cmake/ocs2_cxx_flags.cmake # 全局 C++17 编译标志（供所有包继承）
│ ├── include/ocs2_core/
│ │ ├── Types.h # 全局类型定义(scalar_t, vector_t, matrix_t等)
│ │ ├── augmented_lagrangian/# 增广拉格朗日方法
│ │ ├── automatic_differentiation/ # CppAD/CppADCG 自动微分接口
│ │ ├── constraint/ # 状态约束、状态-输入约束
│ │ ├── control/ # 前馈/线性/状态基控制器
│ │ ├── cost/ # 二次代价函数、代价集合
│ │ ├── dynamics/ # 系统动力学基类、线性化工具
│ │ ├── initialization/ # 轨迹初始化接口
│ │ ├── integration/ # ODE积分器(RK45/敏感性积分)
│ │ ├── loopshaping/ # 回路整形(频域约束转换)
│ │ ├── misc/ # 线性代数工具、插值、日志
│ │ ├── model_data/ # 模型数据结构
│ │ ├── penalties/ # 惩罚函数(松弛障碍/平方合页)
│ │ ├── reference/ # ModeSchedule、目标轨迹
│ │ ├── soft_constraint/ # 软约束包装器
│ │ └── thread_support/ # 线程池、同步原语
│ └── test/ # 单元测试(GTest)
│
├── ocs2_oc/ # 最优控制问题(OCP)抽象层
│ └── include/ocs2_oc/
│ ├── approximate_model/ # 线性二次近似模型
│ ├── multiple_shooting/ # 多重打靶法转录
│ ├── oc_problem/ # OptimalControlProblem 定义
│ ├── oc_solver/ # SolverBase 抽象基类
│ ├── rollout/ # 轨迹前向积分
│ └── synchronized_module/ # ReferenceManager/SolverObserver
│
└── ocs2_thirdparty/ # 第三方库头文件封装
└── include/
├── cppad/ # CppAD 自动微分库（完整头文件）
└── iit/rbd/ # IIT 刚体动力学库
mpc/ — 求解器算法层 ¶
职责：基于ocs2_core + ocs2_oc实现多种 MPC 求解算法。
| 包名 | 算法 | 说明 |
|---|---|---|
ocs2_mpc | MPC 基类 | MPC 框架抽象层与参数设置 |
ocs2_ddp | DDP / SLQ / ILQR | 差分动态规划，含黎卡提方程求解器 |
ocs2_sqp | SQP + HPIPM | 序列二次规划，使用高性能凸 QP 求解器 |
ocs2_ipm | IPM | 内点法，最完整的约束处理能力 |
ocs2_slp | SLP | 序列线性规划 |
ocs2_qp_solver | QP | 基础二次规划求解器 |
ocs2_sqp 包含两个编译子包： - blasfeo_colcon/ — BLASFEO 线性代数库（从源码编译） - hpipm_colcon/ — HPIPM 高性能凸QP求解器（从源码编译）
robotics/ — 机器人接口层 ¶
robotics/
├── ocs2_msgs/ # ROS2 消息与服务定义
│ ├── msg/ # 11个.msg文件
│ │ ├── MpcObservation.msg # 当前状态观测量
│ │ ├── MpcState.msg | MpcInput.msg
│ │ ├── MpcTargetTrajectories.msg
│ │ ├── MpcFlattenedController.msg
│ │ ├── ModeSchedule.msg | MpcPerformanceIndices.msg
│ │ └── Constraint/Multiplier/LagrangianMetrics/ControllerData.msg
│ └── srv/Reset.srv # MPC 重置服务
│
├── ocs2_ros_interfaces/ # ROS2 MPC/MRT 通信接口库
│ ├── include/ocs2_ros_interfaces/
│ │ ├── mpc/ # MPC_ROS_Interface（MPC服务节点）
│ │ ├── mrt/ # MRT_ROS_Interface（模型预测跟踪）
│ │ ├── command/ # 目标轨迹发布器、键盘/交互标记控制
│ │ ├── synchronized_module/ # RosReferenceManager
│ │ └── visualization/ # 可视化辅助工具
│ └── launch/ | multiplot/
│
├── ocs2_pinocchio/ # Pinocchio 刚体动力学集成（5个子包）
│ ├── ocs2_pinocchio_interface/ # Pinocchio 核心接口
│ ├── ocs2_centroidal_model/ # 质心动力学模型（足式机器人专用）
│ ├── ocs2_self_collision/ # 自碰撞检测（基于 hpp-fcl）
│ ├── ocs2_self_collision_visualization/
│ └── ocs2_sphere_approximation/ # 球体近似碰撞模型
│
├── ocs2_robotic_tools/ # 机器人通用工具函数
└── ocs2_python_interface/ # Python-C++ 绑定接口（pybind11）
basic examples/ — 基础示例层 ¶
每类示例分为算法包（纯 C++）和 ROS2 包（含节点 /launch
| 算法包 | ROS2 包 | 控制对象 |
|---|---|---|
ocs2_double_integrator | ocs2_double_integrator_ros | 双积分器（1D 线性系统） |
ocs2_cartpole | ocs2_cartpole_ros | 倒立摆小车（非线性欠驱动） |
ocs2_ballbot | ocs2_ballbot_ros | 球形机器人（3D 非线性平衡） |
ocs2_quadrotor | ocs2_quadrotor_ros | 四旋翼无人机（6-DOF 刚体） |
ocs2_mobile_manipulator | ocs2_mobile_manipulator_ros | 移动机械臂（多种构型） |
ocs2_legged_robot | ocs2_legged_robot_ros | 四足机器人（最复杂基础示例） |
四足机器人包含完整的步态控制系统：
ocs2_legged_robot/
├── config/
│ ├── command/gait.info # 步态模式（trot/walk/stance等）
│ ├── command/reference.info # 参考轨迹配置
│ └── mpc/task.info # MPC任务参数（代价权重/约束）
└── include/ocs2_legged_robot/
├── constraint/ # 摩擦锥/运动学约束
├── foot_planner/ # 落脚点规划（摆动腿轨迹）
├── gait/ # 步态调度器
└── dynamics/ # 质心动力学
移动机械臂支持多种机器人构型： - Franka Panda、Kinova Jaco2、PR2、Ridgeback+UR5、Mabi-Mobile
advance examples/ — 高级示例层 ¶
advance examples/
├── ocs2_perceptive_anymal/ # 感知型四足机器人（Anymal）行走
│ ├── ocs2_switched_model_interface/ # 切换系统模型接口
│ │ └── include/.../
│ │ ├── terrain/ # 地形模型接口（感知地形）
│ │ └── analytical_inverse_kinematics/ # 解析逆运动学
│ ├── ocs2_anymal_models/ # Anymal URDF 与参数
│ ├── ocs2_anymal_mpc/ # Anymal MPC 配置
│ ├── ocs2_anymal_commands/ # 用户指令接口
│ ├── ocs2_quadruped_interface/ # 四足通用接口
│ ├── ocs2_quadruped_loopshaping_interface/ # 回路整形版
│ ├── ocs2_anymal_loopshaping_mpc/ # 回路整形 MPC
│ └── segmented_planes_terrain_model/ # 分段平面地形模型
│
├── ocs2_raisim/ # RaiSim 物理引擎仿真
│ ├── ocs2_raisim_core/ # RaiSim 核心接口适配
│ └── ocs2_legged_robot_raisim/ # 四足机器人完整仿真示例
│ └── launch/legged_robot_ddp_raisim.launch.py
│
└── ocs2_mpcnet/ # MPC-Net 神经网络控制
├── ocs2_mpcnet_core/ # 核心框架（pybind11 + PyTorch + ONNX）
│ └── ocs2_mpcnet_core/
│ ├── mpcnet.py # 训练主逻辑
│ ├── loss/ # 损失函数
│ ├── memory/ # 数据缓冲
│ └── policy/ # 神经网络策略模型
├── ocs2_ballbot_mpcnet/ # Ballbot 的 MPC-Net 示例
└── ocs2_legged_robot_mpcnet/ # 四足机器人的 MPC-Net 示例
submodules/ — 外部子模块层 ¶
submodules/
├── grid_map_sdf/ # 栅格地图符号距离场（SDF）
│ └── include/grid_map_sdf/
│ ├── SignedDistanceField.hpp
│ └── distance_transform/ # 距离变换算法
│
├── ocs2_robotic_assets/ # 机器人资产包（Git子模块，未初始化）
│ └── (URDF/mesh/config 等机器人模型文件，需 git submodule update)
│
└── plane_segmentation_ros2/ # 平面地形分割 ROS2（Git子模块，未初始化）
└── (感知地形分割算法实现，需 git submodule update)
关键配置文件汇总 ¶
| 文件 | 路径 | 说明 |
|---|---|---|
CMakeLists.txt | 每个包根目录（共47 个） | ROS2 ament_cmake 构建配置 |
package.xml | 每个包根目录（共47 个） | ROS2 包元数据与依赖声明 |
ocs2_cxx_flags.cmake | core/ocs2_core/cmake/ | 全局 C++17 编译标志，被所有包继承 |
setup.py | 7 个包 | Python 模块安装配置 |
task.info | 各示例 config/mpc/ | MPC 任务参数（代价矩阵 / 约束参数） |
gait.info | 四足示例 config/command/ | 步态序列定义（trot/walk/stance） |
reference.info | 四足示例 config/command/ | 参考轨迹 / 目标位姿配置 |
requirements.txt | ocs2_mpcnet_core/ | Python ML 依赖（torch/onnx/numpy） |
.gitmodules | 根目录 | 两个子模块声明 |
.clang-tidy | 根目录 | 代码静态分析规则 |
核心设计要点 ¶
-
分层解耦：
core→mpc→robotics→examples严格单向依赖，底层库不依赖上层 -
多求解器支持：同一个控制问题可选 DDP/SQP/IPM 三种求解器（对应各自的 launch 文件）
-
自动微分加速：核心代价 / 约束函数均提供
CppAD版本，支持代码生成（auto_generated/目录缓存） -
MPC-MRT 架构：
ocs2_ros_interfaces实现 MPC 节点（后台优化）与 MRT 节点（实时跟踪）的解耦，适合实时控制部署 -
算法 /ROS 分离：每个示例均分为纯算法包（
ocs2_xxx）和 ROS2 节点包（ocs2_xxx_ros） ，算法包可独立于 ROS 使用 -
Python 生态支持：通过
ocs2_python_interface提供 pybind11 绑定，ocs2_mpcnet实现 PyTorch 训练 + ONNX 部署的完整神经网络控制流水线 -
共 49 个 ROS2 包，全部采用
ament_cmake+colcon构建体系，支持按需选择性编译
子模块初始化说明 ¶
以下两个 Git 子模块当前为空目录，需手动初始化：
git submodule update --init --recursive
submodules/ocs2_robotic_assets→ 提供示例所需的 URDF/mesh 文件submodules/plane_segmentation_ros2→ 提供感知示例的地形分割功能