系列文章目录
前言
一、小车摆杆
推车杆的例子是一个典型的控制问题，一根杆子通过一个无驱动的接头连接到一辆小车上。小车沿着无摩擦轨道移动。目标是在遵守输入限制的前提下，通过沿轨道加速或减速小车，使摆锤从右下方位置开始摆动起来并保持平衡。
# Build the example
catkin build ocs2_cartpole_ros
# Source workspace
# Do not forget to change <...> parts
source <directory_to_ws>/<catkin_ws_name>/devel/setup.bash
# Launch the example
roslaunch ocs2_cartpole_ros cartpole.launch
二、ocs2_cartpole文件夹
2.1 config\mpc\task.info
这段代码定义了一系列参数和配置，用于控制一个名为CartPole的系统的动态行为。其中包括CartPole系统的参数（如质量、长度等），SQP优化算法的配置， rollout（系统动态模拟）的配置，MPC（模型预测控制）的配置，初始状态和状态、控制、目标权重矩阵，以及最终目标状态。这些参数和配置将用于控制CartPole系统的运动。
; cartpole parameters
cartpole_parameters
{
cartMass 2.0
poleMass 0.2
poleLength 1.0
maxInput 5.0
gravity 9.81
}
; DDP settings
ddp
{
algorithm SLQ
nThreads 1
maxNumIterations 1
minRelCost 0.1
constraintTolerance 1e-3
displayInfo false
displayShortSummary false
checkNumericalStability false
AbsTolODE 1e-9
RelTolODE 1e-6
maxNumStepsPerSecond 100000
timeStep 1e-2
backwardPassIntegratorType ODE45
inequalityConstraintMu 100.0
inequalityConstraintDelta 1.1
preComputeRiccatiTerms true
useFeedbackPolicy false
strategy LINE_SEARCH
lineSearch
{
minStepLength 1e-3
maxStepLength 1.0
hessianCorrectionStrategy EIGENVALUE_MODIFICATION
hessianCorrectionMultiple 1e-6
}
}
; Rollout settings
rollout
{
AbsTolODE 1e-9
RelTolODE 1e-6
timeStep 1e-2
maxNumStepsPerSecond 100000
checkNumericalStability false
integratorType ODE45
}
; MPC settings
mpc
{
timeHorizon 5.0 ; [s]
solutionTimeWindow -1 ; maximum [s]
coldStart false
debugPrint false
mpcDesiredFrequency 100 ; [Hz]
mrtDesiredFrequency 400 ; [Hz]
}
bounds_penalty_config
{
scale 0.1
stepSize 1.0
}
; initial state
initialState
{
(0,0) 3.14 ; theta
(1,0) 0.0 ; x
(2,0) 0.0 ; theta_dot
(3,0) 0.0 ; x_dot
}
; state weight matrix
Q
{
(0,0) 0.0 ; theta
(1,1) 0.0 ; x
(2,2) 0.0 ; theta_dot
(3,3) 0.0 ; x_dot
}
; control weight matrix
R
{
(0,0) 0.1
}
; final state weight matrix
Q_final
{
(0,0) 5.0 ; theta
(1,1) 1.0 ; x
(2,2) 1.0 ; theta_dot
(3,3) 1.0 ; x_dot
}
; final goal
x_final
{
(0,0) 0.0 ; theta
(1,0) 0.0 ; x
(2,0) 0.0 ; theta_dot
(3,0) 0.0 ; x_dot
}
这段代码描述了一个模型预测控制（MPC）问题的详细配置，应用在经典的 CartPole 平衡任务上。具体解释如下：
-
cartpole_parameters：这部分定义了CartPole系统的物理参数，如小车质量、杆子质量、杆长、最大输入力、重力加速度等。
-
ddp部分：这是对动态规划算法（特别是SLQ算法）的设定。包括线程数量、最大迭代次数、收敛条件、显示信息选项、数值稳定性检查以及求解微分方程的精度要求和时间步长等。同时，还设定了不等式约束的相关参数，预计算Riccati矩阵的选项，反馈策略，以及线搜索策略的具体参数。
-
rollout部分：定义了在模拟系统动态时使用的欧拉积分器类型及相关的精度需求、时间步长、每秒最大步数以及数值稳定性检查。
-
mpc部分：针对模型预测控制的设置，包含时间窗口长度（预测时域）、是否冷启动、调试打印选项、期望的MPC频率以及实时更新频率等。
-
bounds_penalty_config：边界惩罚系数的配置，包括惩罚尺度和步长。
-
initialState：定义了CartPole系统的初始状态，包括杆角度、小车位置及其速度。
-
Q, R, Q_final：分别代表状态成本矩阵、控制成本矩阵和最终状态的成本矩阵，用于在优化过程中衡量各状态变量和控制变量的重要性或目标偏差的成本。
-
x_final：定义了CartPole系统期望达到的最终稳定状态。
通过上述参数和配置，可以有效地解决CartPole平衡问题，即通过调整小车推力来使杆子在指定时间内保持直立并到达特定的平衡状态。
2.2 config\multiplot\mpc_metrics.xml
以上XML代码描述了一个包含两个曲线的图表配置信息。每个曲线都有自己的一套坐标轴、颜色、数据源和样式设置。整个图表还包含了图例以及标题元素。数据是以时间作为横坐标，对于每一个时间点，都绘制了两条曲线，分别代表某个约束条件或拉格朗日量。文件中详细指定了坐标轴的各项属性，包括坐标轴标题以及最小值和最大值等范围设定。曲线的样式可以自定义，如是否进行线性插值、画笔风格、线条宽度等参数。
<?xml version="1.0" encoding="UTF-8"?>
<rqt_multiplot>
<table>
<background_color>#ffffff</background_color>
<foreground_color>#000000</foreground_color>
<link_cursor>false</link_cursor>
<link_scale>false</link_scale>
<plots>
<row_0>
<column_0>
<axes>
<axes>
<x_axis>
<custom_title>Untitled Axis</custom_title>
<title_type>0</title_type>
<title_visible>true</title_visible>
</x_axis>
<y_axis>
<custom_title>Untitled Axis</custom_title>
<title_type>0</title_type>
<title_visible>true</title_visible>
</y_axis>
</axes>
</axes>
<curves>
<curve_0>
<axes>
<x_axis>
<field>time</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/metrics/InputLimits/0MsLookAhead</topic>
<type>ocs2_msgs/metrics</type>
</x_axis>
<y_axis>
<field>constraint/0</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/metrics/InputLimits/0MsLookAhead</topic>
<type>ocs2_msgs/metrics</type>
</y_axis>
</axes>
<color>
<custom_color>#000000</custom_color>
<type>0</type>
</color>
<data>
<circular_buffer_capacity>1000</circular_buffer_capacity>
<time_frame_length>10</time_frame_length>
<type>0</type>
</data>
<style>
<lines_interpolate>false</lines_interpolate>
<pen_style>1</pen_style>
<pen_width>1</pen_width>
<render_antialias>false</render_antialias>
<steps_invert>false</steps_invert>
<sticks_baseline>0</sticks_baseline>
<sticks_orientation>2</sticks_orientation>
<type>0</type>
</style>
<subscriber_queue_size>100</subscriber_queue_size>
<title>Constraint: u + u_{max} >= 0</title>
</curve_0>
<curve_1>
<axes>
<x_axis>
<field>time</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/multipliers/InputLimits/0MsLookAhead</topic>
<type>ocs2_msgs/multiplier</type>
</x_axis>
<y_axis>
<field>lagrangian/0</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/multipliers/InputLimits/0MsLookAhead</topic>
<type>ocs2_msgs/multiplier</type>
</y_axis>
</axes>
<color>
<custom_color>#000000</custom_color>
<type>0</type>
</color>
<data>
<circular_buffer_capacity>1000</circular_buffer_capacity>
<time_frame_length>10</time_frame_length>
<type>0</type>
</data>
<style>
<lines_interpolate>false</lines_interpolate>
<pen_style>1</pen_style>
<pen_width>1</pen_width>
<render_antialias>false</render_antialias>
<steps_invert>false</steps_invert>
<sticks_baseline>0</sticks_baseline>
<sticks_orientation>2</sticks_orientation>
<type>0</type>
</style>
<subscriber_queue_size>100</subscriber_queue_size>
<title>Lagrangian</title>
</curve_1>
</curves>
<legend>
<visible>true</visible>
</legend>
<plot_rate>30</plot_rate>
<title>TIme 0.0</title>
</column_0>
<column_1>
<axes>
<axes>
<x_axis>
<custom_title>Untitled Axis</custom_title>
<title_type>0</title_type>
<title_visible>true</title_visible>
</x_axis>
<y_axis>
<custom_title>Untitled Axis</custom_title>
<title_type>0</title_type>
<title_visible>true</title_visible>
</y_axis>
</axes>
</axes>
<curves>
<curve_0>
<axes>
<x_axis>
<field>time</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/metrics/InputLimits/0MsLookAhead</topic>
<type>ocs2_msgs/metrics</type>
</x_axis>
<y_axis>
<field>constraint/1</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/metrics/InputLimits/0MsLookAhead</topic>
<type>ocs2_msgs/metrics</type>
</y_axis>
</axes>
<color>
<custom_color>#000000</custom_color>
<type>0</type>
</color>
<data>
<circular_buffer_capacity>1000</circular_buffer_capacity>
<time_frame_length>10</time_frame_length>
<type>0</type>
</data>
<style>
<lines_interpolate>false</lines_interpolate>
<pen_style>1</pen_style>
<pen_width>1</pen_width>
<render_antialias>false</render_antialias>
<steps_invert>false</steps_invert>
<sticks_baseline>0</sticks_baseline>
<sticks_orientation>2</sticks_orientation>
<type>0</type>
</style>
<subscriber_queue_size>100</subscriber_queue_size>
<title>Constraint: -u + u_{max} >= 0</title>
</curve_0>
<curve_1>
<axes>
<x_axis>
<field>time</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/multipliers/InputLimits/0MsLookAhead</topic>
<type>ocs2_msgs/multiplier</type>
</x_axis>
<y_axis>
<field>lagrangian/1</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/multipliers/InputLimits/0MsLookAhead</topic>
<type>ocs2_msgs/multiplier</type>
</y_axis>
</axes>
<color>
<custom_color>#000000</custom_color>
<type>0</type>
</color>
<data>
<circular_buffer_capacity>1000</circular_buffer_capacity>
<time_frame_length>10</time_frame_length>
<type>0</type>
</data>
<style>
<lines_interpolate>false</lines_interpolate>
<pen_style>1</pen_style>
<pen_width>1</pen_width>
<render_antialias>false</render_antialias>
<steps_invert>false</steps_invert>
<sticks_baseline>0</sticks_baseline>
<sticks_orientation>2</sticks_orientation>
<type>0</type>
</style>
<subscriber_queue_size>100</subscriber_queue_size>
<title>Lagrangian</title>
</curve_1>
</curves>
<legend>
<visible>true</visible>
</legend>
<plot_rate>30</plot_rate>
<title>TIme 0.0</title>
</column_1>
</row_0>
<row_1>
<column_0>
<axes>
<axes>
<x_axis>
<custom_title>Untitled Axis</custom_title>
<title_type>0</title_type>
<title_visible>true</title_visible>
</x_axis>
<y_axis>
<custom_title>Untitled Axis</custom_title>
<title_type>0</title_type>
<title_visible>true</title_visible>
</y_axis>
</axes>
</axes>
<curves>
<curve_0>
<axes>
<x_axis>
<field>time</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/metrics/InputLimits/500MsLookAhead</topic>
<type>ocs2_msgs/metrics</type>
</x_axis>
<y_axis>
<field>constraint/0</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/metrics/InputLimits/500MsLookAhead</topic>
<type>ocs2_msgs/metrics</type>
</y_axis>
</axes>
<color>
<custom_color>#000000</custom_color>
<type>0</type>
</color>
<data>
<circular_buffer_capacity>1000</circular_buffer_capacity>
<time_frame_length>10</time_frame_length>
<type>0</type>
</data>
<style>
<lines_interpolate>false</lines_interpolate>
<pen_style>1</pen_style>
<pen_width>1</pen_width>
<render_antialias>false</render_antialias>
<steps_invert>false</steps_invert>
<sticks_baseline>0</sticks_baseline>
<sticks_orientation>2</sticks_orientation>
<type>0</type>
</style>
<subscriber_queue_size>100</subscriber_queue_size>
<title>Constraint: u + u_{max} >= 0</title>
</curve_0>
<curve_1>
<axes>
<x_axis>
<field>time</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/multipliers/InputLimits/500MsLookAhead</topic>
<type>ocs2_msgs/multiplier</type>
</x_axis>
<y_axis>
<field>lagrangian/0</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/multipliers/InputLimits/500MsLookAhead</topic>
<type>ocs2_msgs/multiplier</type>
</y_axis>
</axes>
<color>
<custom_color>#000000</custom_color>
<type>0</type>
</color>
<data>
<circular_buffer_capacity>1000</circular_buffer_capacity>
<time_frame_length>10</time_frame_length>
<type>0</type>
</data>
<style>
<lines_interpolate>false</lines_interpolate>
<pen_style>1</pen_style>
<pen_width>1</pen_width>
<render_antialias>false</render_antialias>
<steps_invert>false</steps_invert>
<sticks_baseline>0</sticks_baseline>
<sticks_orientation>2</sticks_orientation>
<type>0</type>
</style>
<subscriber_queue_size>100</subscriber_queue_size>
<title>Lagrangian</title>
</curve_1>
</curves>
<legend>
<visible>true</visible>
</legend>
<plot_rate>30</plot_rate>
<title>TIme 0.5</title>
</column_0>
<column_1>
<axes>
<axes>
<x_axis>
<custom_title>Untitled Axis</custom_title>
<title_type>0</title_type>
<title_visible>true</title_visible>
</x_axis>
<y_axis>
<custom_title>Untitled Axis</custom_title>
<title_type>0</title_type>
<title_visible>true</title_visible>
</y_axis>
</axes>
</axes>
<curves>
<curve_0>
<axes>
<x_axis>
<field>time</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/metrics/InputLimits/500MsLookAhead</topic>
<type>ocs2_msgs/metrics</type>
</x_axis>
<y_axis>
<field>constraint/1</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/metrics/InputLimits/500MsLookAhead</topic>
<type>ocs2_msgs/metrics</type>
</y_axis>
</axes>
<color>
<custom_color>#000000</custom_color>
<type>0</type>
</color>
<data>
<circular_buffer_capacity>1000</circular_buffer_capacity>
<time_frame_length>10</time_frame_length>
<type>0</type>
</data>
<style>
<lines_interpolate>false</lines_interpolate>
<pen_style>1</pen_style>
<pen_width>1</pen_width>
<render_antialias>false</render_antialias>
<steps_invert>false</steps_invert>
<sticks_baseline>0</sticks_baseline>
<sticks_orientation>2</sticks_orientation>
<type>0</type>
</style>
<subscriber_queue_size>100</subscriber_queue_size>
<title>Constraint: -u + u_{max} >= 0</title>
</curve_0>
<curve_1>
<axes>
<x_axis>
<field>time</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/multipliers/InputLimits/500MsLookAhead</topic>
<type>ocs2_msgs/multiplier</type>
</x_axis>
<y_axis>
<field>lagrangian/1</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/multipliers/InputLimits/500MsLookAhead</topic>
<type>ocs2_msgs/multiplier</type>
</y_axis>
</axes>
<color>
<custom_color>#000000</custom_color>
<type>0</type>
</color>
<data>
<circular_buffer_capacity>1000</circular_buffer_capacity>
<time_frame_length>10</time_frame_length>
<type>0</type>
</data>
<style>
<lines_interpolate>false</lines_interpolate>
<pen_style>1</pen_style>
<pen_width>1</pen_width>
<render_antialias>false</render_antialias>
<steps_invert>false</steps_invert>
<sticks_baseline>0</sticks_baseline>
<sticks_orientation>2</sticks_orientation>
<type>0</type>
</style>
<subscriber_queue_size>100</subscriber_queue_size>
<title>Lagrangian</title>
</curve_1>
</curves>
<legend>
<visible>true</visible>
</legend>
<plot_rate>30</plot_rate>
<title>TIme 0.5</title>
</column_1>
</row_1>
</plots>
<track_points>false</track_points>
</table>
</rqt_multiplot>
这段XML描述了一个包含多条曲线的图表配置信息。该图表被组织为多个“column”和“row”，其中每个“column”内含两个“curve_0”和“curve_1”。对于每一条曲线，其配置细节如下：
-
<axes>: 定义了曲线的坐标轴属性。x_axis中的field表示x轴的数据字段（这里是时间），topic指定了数据来源的主题名称及类型（如ocs2_msgs/metrics或ocs2_msgs/multiplier）。y_axis同样定义了y轴对应的字段和数据源。 -
<scale>: 描述了坐标轴的刻度范围，包括绝对最大值(absolute_maximum)、绝对最小值(absolute_minimum)、相对最大值(relative_maximum)和相对最小值(relative_minimum)，以及刻度类型(type)。 -
<color>: 设置曲线颜色，通过custom_color属性指定十六进制颜色代码，type用于进一步的颜色设置。 -
<data>: 包含数据缓冲区大小(circular_buffer_capacity)和时间窗口长度(time_frame_length)等数据处理参数。 -
<style>: 定义曲线样式，如是否进行线性插值(lines_interpolate)，画笔样式(pen_style)、宽度(pen_width)，是否开启抗锯齿渲染(render_antialias)，步长反转(steps_invert)，基线(sticks_baseline)和方向(sticks_orientation)等。 -
<subscriber_queue_size>: 指定订阅者队列大小。 -
<title>: 为每条曲线提供了标题。
此外，每个"column"还设置了图例(legend)可见性，绘图频率(plot_rate)，以及整体图表标题(title)。
整个XML结构布局清晰，对不同时间段下的约束条件和拉格朗日量进行了可视化配置，以便于展示在不同前瞻时长下的系统性能指标。
2.3 config\multiplot\mpc_observation.xml
这段代码是一个XML格式的多图表绘图配置文件，使用rqt_multiplot工具来展示。配置了一个表单，表单中有一个背景颜色和前景颜色，以及链接曲线和链接比例尺的开关。在图表中绘制了两条曲线，每条曲线都有x轴和y轴的配置，以及曲线的颜色、数据、样式等配置。图表中还有图例和绘图速率的设置。
此XML配置文件用于在rqt_multiplot插件中定义和配置多个数据曲线的二维图表。它包含一个table元素，其中设置了背景颜色、前景颜色、光标链接等全局属性。
<?xml version="1.0" encoding="UTF-8"?>
<rqt_multiplot> # 使用rqt_multiplot插件
<table> # 创建一个表格
<background_color>#ffffff</background_color> # 表格背景颜色为白色
<foreground_color>#000000</foreground_color> # 表格前景颜色为黑色
<link_cursor>false</link_cursor> # 是否连接鼠标
<link_scale>false</link_scale> # 是否连接缩放
<plots> # 绘制多个图表
<row_0> # 第一行为图表的行
<column_0> # 第一列是图表的列
<axes> # 坐标轴
<x_axis> # x轴
<custom_title>Untitled Axis</custom_title> # 自定义标题
<title_type>0</title_type> # 标题类型
<title_visible>true</title_visible> # 标题是否可见
</x_axis>
<y_axis> # y轴
<custom_title>Untitled Axis</custom_title> # 自定义标题
<title_type>0</title_type> # 标题类型
<title_visible>true</title_visible> # 标题是否可见
</y_axis>
</axes>
<curves> # 曲线
<curve_0> # 第一个曲线
<axes> # 坐标轴
<x_axis> # x轴
<field>time</field> # 时间字段
<field_type>0</field_type> # 字段类型
<scale> # 缩放比例
<absolute_maximum>1000</absolute_maximum> # 绝对最大值
<absolute_minimum>0</absolute_minimum> # 绝对最小值
<relative_maximum>0</relative_maximum> # 相对最大值
<relative_minimum>-1000</relative_minimum> # 相对最小值
<type>0</type> # 类型
</scale>
<topic>/cartpole_mpc_observation</topic> # 订阅的话题
<type>ocs2_msgs/mpc_observation</type> # 类型
</x_axis>
<y_axis> # y轴
<field>input/value/0</field> # 输入字段
<field_type>0</field_type> # 字段类型
<scale> # 缩放比例
<absolute_maximum>1000</absolute_maximum> # 绝对最大值
<absolute_minimum>0</absolute_minimum> # 绝对最小值
<relative_maximum>0</relative_maximum> # 相对最大值
<relative_minimum>-1000</relative_minimum> # 相对最小值
<type>0</type> # 类型
</scale>
<topic>/cartpole_mpc_observation</topic> # 订阅的话题
<type>ocs2_msgs/mpc_observation</type> # 类型
</y_axis>
</axes>
<color> # 颜色
<custom_color>#000000</custom_color> # 自定义颜色
<type>0</type> # 类型
</color>
<data> # 数据
<circular_buffer_capacity>1000</circular_buffer_capacity> # 循环缓冲区容量
<time_frame_length>10</time_frame_length> # 时间帧长度
<type>0</type> # 类型
</data>
<style> # 样式
<lines_interpolate>false</lines_interpolate> # 是否插值
<pen_style>1</pen_style> # 笔样式
<pen_width>1</pen_width> # 笔宽度
<render_antialias>false</render_antialias> # 是否反走样
<steps_invert>false</steps_invert> # 是否倒退
<sticks_baseline>0</sticks_baseline> # 是否基线
<sticks_orientation>2</sticks_orientation> # 方向
<type>0</type> # 类型
</style>
<subscriber_queue_size>100</subscriber_queue_size> # 订阅者队列大小
<title>Force</title> # 标题
</curve_0>
</curves>
<legend> # 传入图例
<visible>false</visible> # 是否可见
</legend>
<plot_rate>30</plot_rate> # 绘图速率
<title>Force</title> # 标题
</column_0>
<column_1> # 第一列的第二个图表
<axes> # 坐标轴
<x_axis> # x轴
<custom_title>Untitled Axis</custom_title> # 自定义标题
<title_type>0</title_type> # 标题类型
<title_visible>true</title_visible> # 标题是否可见
</x_axis>
<y_axis> # y轴
<custom_title>Untitled Axis</custom_title> # 自定义标题
<title_type>0</title_type> # 标题类型
<title_visible>true</title_visible> # 标题是否可见
</y_axis>
</axes>
<curves> # 曲线
<curve_0> # 第一个曲线
<axes> # 坐标轴
<x_axis> # x轴
<field>time</field> # 时间字段
<field_type>0</field_type> # 字段类型
<scale> # 缩放比例
<absolute_maximum>1000</absolute_maximum> # 绝对最大值
<absolute_minimum>0</absolute_minimum> # 绝对最小值
<relative_maximum>0</relative_maximum> # 相对最大值
<relative_minimum>-1000</relative_minimum> # 相对最小值
<type>0</type> # 类型
</scale>
<topic>/cartpole_mpc_observation</topic> # 订阅的话题
<type>ocs2_msgs/mpc_observation</type> # 类型
</x_axis>
<y_axis> # y轴
<field>state/value/0</field> # 状态字段
<field_type>0</field_type> # 字段类型
<scale> # 缩放比例
<absolute_maximum>1000</absolute_maximum> # 绝对最大值
<absolute_minimum>0</absolute_minimum> # 绝对最小值
<relative_maximum>0</relative_maximum> # 相对最大值
<relative_minimum>-1000</relative_minimum> # 相对最小值
<type>0</type> # 类型
</scale>
<topic>/cartpole_mpc_observation</topic> # 订阅的话题
<type>ocs2_msgs/mpc_observation</type> # 类型
</y_axis>
</axes>
<color> # 颜色
<custom_color>#000000</custom_color> # 自定义颜色
<type>0</type> # 类型
</color>
<data> # 数据
<circular_buffer_capacity>1000</circular_buffer_capacity> # 循环缓冲区容量
<time_frame_length>10</time_frame_length> # 时间帧长度
<type>0</type> # 类型
</data>
<style> # 样式
<lines_interpolate>false</lines_interpolate> # 是否插值
<pen_style>1</pen_style> # 笔样式
<pen_width>1</pen_width> # 笔宽度
<render_antialias>false</render_antialias> # 是否反走样
<steps_invert>false</steps_invert> # 是否倒退
<sticks_baseline>0</sticks_baseline> # 是否基线
<sticks_orientation>2</sticks_orientation> # 方向
<type>0</type> # 类型
</style>
<subscriber_queue_size>100</subscriber_queue_size> # 订阅者队列大小
<title>Pendulum Angle</title> # 标题
</curve_0>
</curves>
<legend> # 传入图例
<visible>false</visible> # 是否可见
</legend>
<plot_rate>30</plot_rate> # 绘图速率
<title>Pendulum Angle</title> # 标题
</column_1>
</row_0>
<row_1> # 第二行
<column_0> # 第一列
<axes> # 坐标轴
<x_axis> # x轴
<custom_title>Untitled Axis</custom_title> # 自定义标题
<title_type>0</title_type> # 标题类型
<title_visible>true</title_visible> # 标题是否可见
</x_axis>
<y_axis> # y轴
<custom_title>Untitled Axis</custom_title> # 自定义标题
<title_type>0</title_type> # 标题类型
<title_visible>true</title_visible> # 标题是否可见
</y_axis>
</axes>
<curves> # 曲线
<curve_0> # 第一个曲线
<axes> # 坐标轴
<x_axis> # x轴
<field>time</field> # 时间字段
<field_type>0</field_type> # 字段类型
<scale> # 缩放比例
<absolute_maximum>1000</absolute_maximum> # 绝对最大值
<absolute_minimum>0</absolute_minimum> # 绝对最小值
<relative_maximum>0</relative_maximum> # 相对最大值
<relative_minimum>-1000</relative_minimum> # 相对最小值
<type>0</type> # 类型
</scale>
<topic>/cartpole_mpc_observation</topic> # 订阅的话题
<type>ocs2_msgs/mpc_observation</type> # 类型
</x_axis>
<y_axis> # y轴
<field>state/value/1</field> # 状态字段
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/cartpole_mpc_observation</topic>
<type>ocs2_msgs/mpc_observation</type>
</y_axis>
</axes>
<color>
<custom_color>#000000</custom_color>
<type>0</type>
</color>
<data>
<circular_buffer_capacity>1000</circular_buffer_capacity>
<time_frame_length>10</time_frame_length>
<type>0</type>
</data>
<style>
<lines_interpolate>false</lines_interpolate>
<pen_style>1</pen_style>
<pen_width>1</pen_width>
<render_antialias>false</render_antialias>
<steps_invert>false</steps_invert>
<sticks_baseline>0</sticks_baseline>
<sticks_orientation>2</sticks_orientation>
<type>0</type>
</style>
<subscriber_queue_size>100</subscriber_queue_size>
<title>Cart Position</title>
</curve_0>
</curves>
<legend>
<visible>false</visible>
</legend>
<plot_rate>30</plot_rate>
<title>Cart Position</title>
</column_0>
<column_1>
<axes>
<axes>
<x_axis>
<custom_title>Untitled Axis</custom_title>
<title_type>0</title_type>
<title_visible>true</title_visible>
</x_axis>
<y_axis>
<custom_title>Untitled Axis</custom_title>
<title_type>0</title_type>
<title_visible>true</title_visible>
</y_axis>
</axes>
</axes>
<curves>
<curve_0>
<axes>
<x_axis>
<field>time</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/cartpole_mpc_observation</topic>
<type>ocs2_msgs/mpc_observation</type>
</x_axis>
<y_axis>
<field>state/value/2</field>
<field_type>0</field_type>
<scale>
<absolute_maximum>1000</absolute_maximum>
<absolute_minimum>0</absolute_minimum>
<relative_maximum>0</relative_maximum>
<relative_minimum>-1000</relative_minimum>
<type>0</type>
</scale>
<topic>/cartpole_mpc_observation</topic>
<type>ocs2_msgs/mpc_observation</type>
</y_axis>
</axes>
<color>
<custom_color>#000000</custom_color>
<type>0</type>
</color>
<data>
<circular_buffer_capacity>1000</circular_buffer_capacity>
<time_frame_length>10</time_frame_length>
<type>0</type>
</data>
<style>
<lines_interpolate>false</lines_interpolate>
<pen_style>1</pen_style>
<pen_width>1</pen_width>
<render_antialias>false</render_antialias>
<steps_invert>false</steps_invert>
<sticks_baseline>0</sticks_baseline>
<sticks_orientation>2</sticks_orientation>
<type>0</type>
</style>
<subscriber_queue_size>100</subscriber_queue_size>
<title>Cart Velocity</title>
</curve_0>
</curves>
<legend>
<visible>false</visible>
</legend>
<plot_rate>30</plot_rate>
<title>Cart Velocity</title>
</column_1>
</row_1>
</plots>
<track_points>false</track_points>
</table>
</rqt_multiplot> 该配置详细描述了两个行(row)和每行两个列(column)的图表布局：
-
每个
column_X元素代表一个单独的图表，并设置了其标题(title)、更新频率(plot_rate)以及是否显示图例(legend/visible)。 -
在每个图表内部，
axes元素定义了x轴和y轴的配置，包括自定义标题、可见性等属性； -
curves元素下包含了一个名为curve_0的子元素，用于定义实际的数据曲线。对于每条曲线：axes内指定了x轴和y轴所对应的数据字段(field)、话题(topic)以及数据类型(type)，并设置了坐标轴的刻度范围(scale)；color元素定义了曲线的颜色；data元素包含了缓冲区大小(circular_buffer_capacity)和时间帧长度(time_frame_length)；style元素详细规定了曲线的样式属性，如是否平滑插值(lines_interpolate)、线宽(pen_width)、点划线风格(pen_style)等；subscriber_queue_size定义了订阅者队列大小；title元素给出了曲线的标题。
该配置文件从/cartpole_mpc_observation话题获取ocs2_msgs/mpc_observation消息类型的数据，并绘制了四条不同的曲线：力（Force）、摆杆角度（Pendulum Angle）、小车位置（Cart Position）和小车速度（Cart Velocity）。
2.4 include\ocs2_cartpole\dynamics\CartPoleSystemDynamics.h
这是一个表示购物车摆杆系统动力学的C++类。它继承了SystemDynamicsBaseAD类，并实现了systemFlowMap函数用于计算系统的状态流映射。该函数考虑了系统的质量和转动惯量，并根据给定的状态和输入计算系统的状态导数。
/*******************************************************************************
Copyright (c) 2017, Farbod Farshidian. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/
#pragma once
#include <ocs2_core/dynamics/SystemDynamicsBaseAD.h>
#include "ocs2_cartpole/CartPoleParameters.h"
#include "ocs2_cartpole/definitions.h"
namespace ocs2 {
namespace cartpole {
/**
* @brief CartPole系统的动力学。
* 参考：https://pdfs.semanticscholar.org/f95b/9d4cc0814034f2e601cb91fcd70b2e806420.pdf
*/
class CartPoleSytemDynamics : public SystemDynamicsBaseAD {
public:
/**
* @brief 构造函数。
* @param cartPoleParameters CartPole参数。
* @param libraryFolder 动力学库文件夹路径。
* @param verbose 是否显示详细信息。
*/
CartPoleSytemDynamics(const CartPoleParameters& cartPoleParameters, const std::string& libraryFolder, bool verbose)
: param_(cartPoleParameters) {
initialize(STATE_DIM, INPUT_DIM, "cartpole_dynamics", libraryFolder, true, verbose);
}
/**
* @brief 析构函数。
*/
~CartPoleSytemDynamics() override = default;
/**
* @brief 拷贝构造函数。
* @param rhs 右操作数。
*/
CartPoleSytemDynamics(const CartPoleSytemDynamics& rhs) = default;
/**
* @brief 克隆函数。
* @return 克隆对象指针。
*/
CartPoleSytemDynamics* clone() const override { return new CartPoleSytemDynamics(*this); }
/**
* @brief 系统流映射函数。
* @param time 时间。
* @param state 状态向量。
* @param input 控制输入向量。
* @param parameters 参数向量。
* @return 状态导数向量。
*/
ad_vector_t systemFlowMap(ad_scalar_t time, const ad_vector_t& state, const ad_vector_t& input,
const ad_vector_t& parameters) const override {
const ad_scalar_t cosTheta = cos(state(0));
const ad_scalar_t sinTheta = sin(state(0));
// Inertia tensor
Eigen::Matrix<ad_scalar_t, 2, 2> I;
I << static_cast<ad_scalar_t>(param_.poleSteinerMoi_), static_cast<ad_scalar_t>(param_.poleMass_ * param_.poleHalfLength_ * cosTheta),
static_cast<ad_scalar_t>(param_.poleMass_ * param_.poleHalfLength_ * cosTheta),
static_cast<ad_scalar_t>(param_.cartMass_ + param_.poleMass_);
// RHS
Eigen::Matrix<ad_scalar_t, 2, 1> rhs(param_.poleMass_ * param_.poleHalfLength_ * param_.gravity_ * sinTheta,
input(0) + param_.poleMass_ * param_.poleHalfLength_ * pow(state(2), 2) * sinTheta);
// dxdt
ad_vector_t stateDerivative(STATE_DIM);
stateDerivative << state.tail<2>(), I.inverse() * rhs;
return stateDerivative;
}
private:
CartPoleParameters param_;
};
} // namespace cartpole
} // namespace ocs2该C++代码定义了一个名为CartPoleSytemDynamics的类，它继承自ocs2::SystemDynamicsBaseAD基类，并用于描述物理系统的动态行为，具体来说是一个带有摆杆的购物车系统。此类主要用于计算系统在给定时间、状态和输入下的状态导数（微分方程的右端项）。
-
构造函数：接收一个
CartPoleParameters对象（包含系统参数如质量、重力加速度等）和两个字符串参数（库文件夹路径和是否开启详细模式），并调用基类方法进行初始化。 -
成员变量：
param_为一个CartPoleParameters类型的对象，存储了系统的具体参数。 -
systemFlowMap函数是关键成员函数，根据牛顿力学原理计算出系统状态的时间导数。它首先计算了一些辅助变量，如角度的正弦值和余弦值，然后构建惯性张量矩阵I。接下来，根据系统动力学公式计算RHS，最后利用逆惯性张量求解得到状态导数。 -
其他函数（如默认构造函数、复制构造函数和clone函数）提供了对象创建和拷贝的能力，遵循C++面向对象编程的基本原则。
总之，这个类为基于自动微分（AD）的购物车摆杆系统提供了一种动力学模型实现，可用于控制系统设计与分析。
2.5 include\ocs2_cartpole\CartPoleInterface.h
这段C++代码定义了一个名为CartPoleInterface的类的构造函数。这个构造函数接受三个参数：taskFile（一个表示MPC配置文件路径的字符串），libraryFolder（一个表示将要生成CppAD库的目标目录路径的字符串）以及verbose（一个布尔标志，表示是否打印出设置信息和库编译状态）。通过该构造函数，初始化CartPoleInterface对象时会设置初始状态、最终目标状态、DDP设置、MPC设置、最优控制问题、实施策略和初始化器等属性。
/*******************************************************************************
Copyright (c) 2020, Farbod Farshidian. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
#pragma once
// OCS2
#include <ocs2_core/Types.h>
#include <ocs2_core/initialization/Initializer.h>
#include <ocs2_ddp/DDP_Settings.h>
#include <ocs2_mpc/MPC_Settings.h>
#include <ocs2_oc/rollout/TimeTriggeredRollout.h>
#include <ocs2_robotic_tools/common/RobotInterface.h>
// CartPole
#include "ocs2_cartpole/CartPoleParameters.h"
#include "ocs2_cartpole/definitions.h"
namespace ocs2 {
namespace cartpole {
class CartPoleInterface final : public RobotInterface {
public:
/**
* 构造函数
*
* @note 创建生成库所在目录（如果不存在的话）。
* @throw Invalid argument error 如果输入的MPC配置文件不存在。
*
* @param [in] taskFile: MPC的配置文件的绝对路径。
* @param [in] libraryFolder: 生成库的绝对路径。
* @param [in] verbose: 标志位，用于确定是否打印出优化库的设置和编译库的状态。
*/
CartPoleInterface(const std::string& taskFile, const std::string& libraryFolder, bool verbose);
/**
* 析构函数
*/
~CartPoleInterface() override = default;
const vector_t& getInitialState() { return initialState_; }
const vector_t& getInitialTarget() { return xFinal_; }
ddp::Settings& ddpSettings() { return ddpSettings_; }
mpc::Settings& mpcSettings() { return mpcSettings_; }
OptimalControlProblem& optimalControlProblem() { return problem_; }
const OptimalControlProblem& getOptimalControlProblem() const override { return problem_; }
const RolloutBase& getRollout() const { return *rolloutPtr_; }
const Initializer& getInitializer() const override { return *cartPoleInitializerPtr_; }
private:
ddp::Settings ddpSettings_;
mpc::Settings mpcSettings_;
OptimalControlProblem problem_;
std::unique_ptr<RolloutBase> rolloutPtr_;
std::unique_ptr<Initializer> cartPoleInitializerPtr_;
vector_t initialState_{STATE_DIM};
vector_t xFinal_{STATE_DIM};
};
} // namespace cartpole
} // namespace ocs2这段C++代码定义了一个名为CartPoleInterface的类，该类主要用于实现对倒立摆系统的接口操作。它继承自RobotInterface基类，并在其内部包含了与倒立摆系统相关的各种设置和数据结构。
构造函数CartPoleInterface接受三个参数：
taskFile：一个字符串类型的参数，表示MPC（Model Predictive Control）配置文件的绝对路径。libraryFolder：一个字符串类型的参数，指定了生成CppAD库（用于自动微分计算）的目标目录的绝对路径。verbose：一个布尔类型的参数，若为真，则在初始化过程中打印出加载的设置信息以及编译后的库的状态信息。
成员函数包括：
- 获取初始状态：通过
getInitialState()方法获取系统的初始状态向量。 - 获取最终目标状态：通过
getInitialTarget()方法获取系统的期望终止状态向量。 - 访问DDP和MPC相关设置：分别通过
ddpSettings()和mpcSettings()方法来获取或修改DDP算法和MPC算法的相关设置。 - 获取最优控制问题模型：通过
optimalControlProblem()方法返回一个引用，指向内部存储的最优控制问题对象；同时提供了一个重载版本的getOptimalControlProblem()，符合基类RobotInterface的要求。 - 获取执行策略（Rollout）：通过
getRollout()方法获取一个指向执行策略对象的常量引用。 - 获取初始化器：通过
getInitializer()方法获取一个指向初始化器对象的常量引用，符合基类RobotInterface的要求。
类的私有成员变量中，包含了DDP和MPC的设置、最优控制问题实例、执行策略（RolloutBase）的智能指针、初始化器（Initializer）的智能指针，以及系统初始状态向量和期望终止状态向量。
2.6 include\ocs2_cartpole\CartPoleParameters.h
这段代码定义了一个结构体CartPoleParameters，用于存储CartPole系统的参数。结构体中包含了CartPole系统的各种参数，如质量、长度、最大输入等。该结构体还提供了构造函数、显示参数和从文件加载参数的方法。加载参数方法通过Boost库的property_tree解析参数文件，并支持输出参数。最后，该结构体还包含了计算惯性项的方法。
/*******************************************************************************
Copyright (c) 2017, Farbod Farshidian. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
#pragma once
#include <iostream>
#include <string>
#include <vector>
#include <ocs2_core/Types.h>
#include <ocs2_core/misc/LoadData.h>
namespace ocs2 {
namespace cartpole {
// 车轮杆模型的参数结构体
struct CartPoleParameters {
/** 构造函数 */
CartPoleParameters() { computeInertiaTerms(); }
/** 显示参数 */
void display() {
std::cerr << "Cart-pole parameters: "
<< "\n";
std::cerr << "cartMass: " << cartMass_ << "\n";
std::cerr << "poleMass: " << poleMass_ << "\n";
std::cerr << "poleLength: " << poleLength_ << "\n";
std::cerr << "poleMoi: " << poleMoi_ << "\n";
std::cerr << "maxInput: " << maxInput_ << "\n";
std::cerr << "gravity: " << gravity_ << "\n";
}
/** 加载车轮杆模型的参数 */
void loadSettings(const std::string& filename, const std::string& fieldName, bool verbose = true) {
boost::property_tree::ptree pt;
boost::property_tree::read_info(filename, pt);
if (verbose) {
std::cerr << "\n #### Cart-pole Parameters:";
std::cerr << "\n #### =============================================================================\n";
}
loadData::loadPtreeValue(pt, cartMass_, fieldName + ".cartMass", verbose);
loadData::loadPtreeValue(pt, poleMass_, fieldName + ".poleMass", verbose);
loadData::loadPtreeValue(pt, poleLength_, fieldName + ".poleLength", verbose);
loadData::loadPtreeValue(pt, maxInput_, fieldName + ".maxInput", verbose);
loadData::loadPtreeValue(pt, gravity_, fieldName + ".gravity", verbose);
computeInertiaTerms();
if (verbose) {
std::cerr << " #### =============================================================================\n" << std::endl;
}
}
scalar_t cartMass_ = 1.0; // [kg], 车的质量
scalar_t poleMass_ = 1.0; // [kg], 杆的质量
scalar_t poleLength_ = 1.0; // [m], 杆的长度
scalar_t maxInput_ = 6.0; // [N], 输入的最大值
scalar_t gravity_ = 9.8; // [m/s^2], 重力加速度
scalar_t poleHalfLength_ = -1; // [m], 杆的一半长度
scalar_t poleMoi_ = -1; // [kg*m^2], 杆的转动惯量
scalar_t poleSteinerMoi_ = -1; // [kg*m^2], 杆的质心转动惯量
private:
/** 计算转动惯量相关项 */
void computeInertiaTerms() {
poleHalfLength_ = poleLength_ / 2.0;
poleMoi_ = 1.0 / 12.0 * poleMass_ * (poleLength_ * poleLength_);
poleSteinerMoi_ = poleMoi_ + poleMass_ * (poleHalfLength_ * poleHalfLength_);
}
};
} // namespace cartpole
} // namespace ocs2这段C++代码定义了一个名为CartPoleParameters的结构体，用于存储和操作Cart-Pole模型的相关参数。该模型描述的是一个在平面上可左右移动的小车（cart）上固定着一根可以绕其与小车间连接点旋转的杆子（pole）的经典物理系统。
-
结构体内定义了如下成员变量：
cartMass_: 小车的质量，默认值为1.0 kg。poleMass_: 杆子的质量，默认值为1.0 kg。poleLength_: 杆子的长度，默认值为1.0 m。maxInput_: 控制输入的最大力，默认值为6.0 N。gravity_: 重力加速度，默认值为9.8 m/s^2。poleHalfLength_: 杆子半长，计算得出。poleMoi_: 杆子关于旋转轴的转动惯量，计算得出。poleSteinerMoi_: 杆子包含Steiner转动惯量在内的总转动惯量，计算得出。
-
构造函数：当创建
CartPoleParameters对象时，会自动调用computeInertiaTerms()方法计算杆子相关的转动惯量。 -
display()方法：用于输出所有当前设置的参数值到标准错误流（cerr）。 -
loadSettings()方法：从给定文件名的配置文件中加载Cart-Pole系统的参数。使用Boost库的property_tree来解析ini格式的配置文件，并通过loadData::loadPtreeValue()函数将属性树中的值加载到对应的成员变量中。加载过程可以选择是否显示详细信息。 -
computeInertiaTerms()方法：私有成员函数，用于根据杆子质量和长度计算杆子的半长、转动惯量以及包含Steiner项的转动惯量。
2.7 include\ocs2_cartpole\definitions.h
这段代码定义了一个名为cartpole的命名空间，并在该命名空间中定义了两个常量，STATE_DIM和INPUT_DIM，分别表示状态维度和输入维度的大小。同时包含了一个名为ocs2的命名空间。
/**************************************************************************
*
* Copyright (c) 2017, Farbod Farshidian. All rights reserved.
*
* 源代码的重新分配和使用，以及二进制形式的重新分配和使用，无论是否经过修改，
* 如果满足以下条件，都是被允许的：
*
* * 源代码的重新分配必须保留以上版权声明，此条件列表和以下弃权声明。
*
* * 在文档和/或其他材料中提供的二进制形式的再分配必须复制以上版权声明，
* 此条件列表和以下弃权声明。
*
* * 未获得特定先前书面许可，版权人或贡献者的名字，不得用作批准或推广
* 从该软件派生的产品的 endorsements 或 promotions。
*
* 本软件依照“原样”提供，没有任何明示或暗示的保证，包括但不仅限于
* 商业适销性和特定目的适用性方面的保证。在任何情况下，版权人或
* 贡献者对任何直接，间接，意外，特殊，示例，后果或间接损害（包括但不仅限于
* 购买替代商品或服务的费用；或使用之数据或利润的损失；或业务中断），
* 由于使用或无法使用该软件而产生的情况，无论是在何种法律理论（包括疏忽或其他原因）下，
* 均不受责任。
*
**************************************************************************/
#pragma once
#include <cstddef>
namespace ocs2 {
namespace cartpole {
constexpr size_t STATE_DIM = 4; // 状态维度
constexpr size_t INPUT_DIM = 1; // 输入维度
} // namespace cartpole
} // namespace ocs2这段C++代码包含版权说明和授权许可，声明了在"ocs2"顶级命名空间下的"cartpole"子命名空间。在这个cartpole命名空间中定义了两个constexpr类型的变量：
-
STATE_DIM：表示系统的状态维度，其类型为size_t（无符号整型），在这里它的值被设定为4，意味着系统状态向量有四个元素。 -
INPUT_DIM：表示系统的输入维度，同样类型为size_t，其值为1，表示系统的控制输入是一个单维度的量。
这两个常量通常用于定义控制系统中的状态方程和输入，例如在设计动态模型、仿真器或控制器时，会用到这些维度信息。
2.8 include\ocs2_cartpole\package_path.h.in
该CPP函数是一个在cartpole命名空间下的函数，用于获取包源代码目录的路径。通过使用PROJECT_SOURCE_DIR宏，返回包源代码目录的路径。
// 导入所需的头文件
#include <cstddef>
#include <string>
// 定义命名空间
namespace ocs2 {
namespace cartpole {
/**
* 获取包源代码目录的路径。
*/
inline std::string getPath() {
return "@PROJECT_SOURCE_DIR@";
}
} // namespace cartpole
} // namespace ocs2该CPP函数是定义在ocs2::cartpole命名空间下的一个内联函数，名为getPath()。其主要功能是从预处理器宏@PROJECT_SOURCE_DIR@获取项目源代码的根目录路径，并以字符串形式返回。
当编译项目时，CMake等构建系统会将@PROJECT_SOURCE_DIR@替换为实际的项目源码路径。因此，在运行时调用这个函数可以得到当前项目的源代码所在目录，这对于需要访问项目内部资源文件等情况非常有用。
2.9 src\CartPoleInterface.cpp
这个函数是CartPoleInterface类的一个构造函数，接受三个参数：taskFile、libraryFolder和verbose。它加载一个任务文件，如果不存在则创建一个库文件夹，设置默认初始条件和最终状态，加载DDP-MPC相关设置，定义带有二次状态和输入代价的最优控制问题，使用CartPoleSytemDynamics类设置动力学模型，通过TimeTriggeredRollout类创建一个实施过程，利用LinearStateInputConstraint类定义约束，并使用DefaultInitializer类初始化拉格朗日乘子。
// 版权所有 Farbod Farshidian，2020年。版权全部保留。
// 以下条款必须得到满足的情况下，可以进行再分发和二进制形式的修改：
// 所有上述版权 notice，此条件清单以及随后的弃权声明，必须包含在所有通过任何方式从软件获得的作品中。
// 通过任何方式从软件获得的作品，包括经过修改的版本，若要用来促进或促销产品，必须得到特定的书面许可。
// 本软件提供“原样”，没有任何种类的明确或暗示的保证，包括但不仅限于适销性、特定用途之适用性及非侵权。
// 在任何情况下，作者或贡献者不对任何间接、附带、偶然、特殊或后果性损害负责，即使已被告知有可能发生此类损害。
// 此软件的使用者和/或使用者的继承人、执行者、转让者和分让人在此明示放弃对作者和/或贡献者的任何种类的诉讼、索赔或义务的任何权利、利益或救济，包括但不仅限于根据合同、严格责任或侵权行为而产生的任何损害。
#include <iostream>
#include <string>
#include "ocs2_cartpole/CartPoleInterface.h"
#include "ocs2_cartpole/dynamics/CartPoleSystemDynamics.h"
#include <ocs2_core/augmented_lagrangian/AugmentedLagrangian.h>
#include <ocs2_core/constraint/LinearStateInputConstraint.h>
#include <ocs2_core/cost/QuadraticStateCost.h>
#include <ocs2_core/cost/QuadraticStateInputCost.h>
#include <ocs2_core/initialization/DefaultInitializer.h>
#include <ocs2_core/misc/LoadData.h>
#include <ocs2_core/penalties/Penalties.h>
// Boost
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>
namespace ocs2 {
namespace cartpole {
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
// 构造函数
CartPoleInterface::CartPoleInterface(const std::string& taskFile, const std::string& libraryFolder, bool verbose) {
// 检查任务文件是否存在
boost::filesystem::path taskFilePath(taskFile);
if (boost::filesystem::exists(taskFilePath)) {
std::cerr << "[CartPoleInterface] 正在加载任务文件: " << taskFilePath << "\n";
} else {
throw std::invalid_argument("[CartPoleInterface] 未找到任务文件: " + taskFilePath.string());
}
// 创建库文件夹（如果不存在）
boost::filesystem::path libraryFolderPath(libraryFolder);
boost::filesystem::create_directories(libraryFolderPath);
std::cerr << "[CartPoleInterface] 生成的库路径: " << libraryFolderPath << "\n";
// 默认初始条件
loadData::loadEigenMatrix(taskFile, "initialState", initialState_);
loadData::loadEigenMatrix(taskFile, "x_final", xFinal_);
if (verbose) {
std::cerr << "x_init: " << initialState_.transpose() << "\n";
std::cerr << "x_final: " << xFinal_.transpose() << "\n";
}
// DDP-MPC 设置
ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose);
mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose);
/*
* 最优控制问题
*/
// 代价函数
matrix_t Q(STATE_DIM, STATE_DIM);
matrix_t R(INPUT_DIM, INPUT_DIM);
matrix_t Qf(STATE_DIM, STATE_DIM);
loadData::loadEigenMatrix(taskFile, "Q", Q);
loadData::loadEigenMatrix(taskFile, "R", R);
loadData::loadEigenMatrix(taskFile, "Q_final", Qf);
if (verbose) {
std::cerr << "Q: \n" << Q << "\n";
std::cerr << "R: \n" << R << "\n";
std::cerr << "Q_final:\n" << Qf << "\n";
}
problem_.costPtr->add("cost", std::make_unique<QuadraticStateInputCost>(Q, R));
problem_.finalCostPtr->add("finalCost", std::make_unique<QuadraticStateCost>(Qf));
// 动态
CartPoleParameters cartPoleParameters;
cartPoleParameters.loadSettings(taskFile, "cartpole_parameters", verbose);
problem_.dynamicsPtr.reset(new CartPoleSytemDynamics(cartPoleParameters, libraryFolder, verbose));
// 时间触发的 rollout
auto rolloutSettings = rollout::loadSettings(taskFile, "rollout", verbose);
rolloutPtr_.reset(new TimeTriggeredRollout(*problem_.dynamicsPtr, rolloutSettings));
// 约束
auto getPenalty = [&]() {
// 可以使用 augmented::SlacknessSquaredHingePenalty 或 augmented::ModifiedRelaxedBarrierPenalty
using penalty_type = augmented::SlacknessSquaredHingePenalty;
penalty_type::Config boundsConfig;
loadData::loadPenaltyConfig(taskFile, "bounds_penalty_config", boundsConfig, verbose);
return penalty_type::create(boundsConfig);
};
auto getConstraint = [&]() {
constexpr size_t numIneqConstraint = 2;
const vector_t e = (vector_t(numIneqConstraint) << cartPoleParameters.maxInput_, cartPoleParameters.maxInput_).finished();
const vector_t D = (vector_t(numIneqConstraint) << 1.0, -1.0).finished();
const matrix_t C = matrix_t::Zero(numIneqConstraint, STATE_DIM);
return std::make_unique<LinearStateInputConstraint>(e, C, D);
};
problem_.inequalityLagrangianPtr->add("InputLimits", create(getConstraint(), getPenalty()));
// 初始化
cartPoleInitializerPtr_.reset(new DefaultInitializer(INPUT_DIM));
}
} // namespace cartpole
} // namespace ocs2这个函数是CartPoleInterface类的一个构造函数，接受三个参数：taskFile、libraryFolder和verbose。其功能主要包括：
- 检查并加载指定的taskFile任务文件，如果文件不存在则抛出异常。
- 创建并确保libraryFolder路径存在。
- 设置默认的初始条件和最终状态，并从taskFile中加载这些数据。
- 从taskFile加载DDP-MPC的相关设置。
- 定义最优控制问题，其中代价函数包含状态和输入的二次成本项，从taskFile加载Q、R和Qf矩阵以定义这些成本项。
- 使用CartPoleSytemDynamics类设置系统的动力学模型。
- 根据taskFile创建一个基于时间触发的 rollout策略实例（TimeTriggeredRollout）。
- 从taskFile加载约束信息，使用LinearStateInputConstraint类定义不等式约束，并通过SlacknessSquaredHingePenalty或ModifiedRelaxedBarrierPenalty计算惩罚项。
- 初始化拉格朗日乘子，使用DefaultInitializer类进行处理。
整体而言，该构造函数用于初始化一个解决倒立摆问题的最优控制框架所需的各项参数与组件。
2.10 test\testCartpole.cpp
该cpp函数是一个测试函数，用于测试优化问题的求解器。它使用Google Test框架来执行单元测试，并在每个测试用例中使用不同的算法和惩罚项来解决相同的优化问题。测试函数包括设置测试环境、创建求解器、运行求解器和测试结果的步骤。测试的指标包括目标状态的误差和约束的violations。
// 引入必要的C++库和OCS2相关头文件，包括gtest测试框架、ocs2核心库、动态规划算法模块以及同步模块等
#include <cmath>
#include <iostream>
#include <string>
#include <thread>
// 引入Google Test单元测试框架
#include <gtest/gtest.h>
// 引入OCS2核心库的相关头文件
#include <ocs2_core/augmented_lagrangian/AugmentedLagrangian.h>
#include <ocs2_core/cost/QuadraticStateCost.h>
#include <ocs2_core/penalties/Penalties.h>
// 引入OCS2动态规划算法模块（如ILQR和SLQ）的头文件
#include <ocs2_ddp/ILQR.h>
#include <ocs2_ddp/SLQ.h>
// 引入OCS2同步模块的头文件
#include <ocs2_oc/synchronized_module/SolverObserver.h>
// 引入库特定接口与路径相关的头文件，这里针对的是CartPole问题
#include "ocs2_cartpole/CartPoleInterface.h"
#include "ocs2_cartpole/package_path.h"
using namespace ocs2;
using namespace cartpole;
// 定义惩罚类型枚举类，用于指定不同类型的约束惩罚方法
enum class PenaltyType {
SlacknessSquaredHingePenalty,
ModifiedRelaxedBarrierPenalty,
};
// 创建一个测试类TestCartpole，继承自gtest的TestWithParam模板类，以便进行参数化测试
class TestCartpole : public testing::TestWithParam<std::tuple<ddp::Algorithm, PenaltyType>> {
public:
// 声明静态常量成员变量，定义时间范围、目标违反容忍度和约束违反容忍度
static constexpr scalar_t timeHorizon = 15.0; // 时间范围（秒）
static constexpr scalar_t goalViolationTolerance = 1e-1; // 目标状态偏离容忍度
static constexpr scalar_t constraintViolationTolerance = 1e-1; // 约束违反容忍度
// 构造函数，在初始化时设置环境配置信息并准备最优控制问题
TestCartpole() {
// 初始化任务文件和自动生成库文件夹的路径
taskFile = ocs2::cartpole::getPath() + "/config/mpc/task.info";
const std::string libFolder = ocs2::cartpole::getPath() + "/auto_generated";
// 创建并初始化CartPoleInterface对象，它封装了CartPole系统的模型和优化问题配置
cartPoleInterfacePtr.reset(new CartPoleInterface(taskFile, libFolder, false /*verbose*/));
// 调整最终成本以适应给定的时间范围，并将其添加到最优控制问题中
const std::string finalCostName = "finalCost";
if (!cartPoleInterfacePtr->optimalControlProblem().finalCostPtr->erase(finalCostName)) {
throw std::runtime_error("[TestCartpole::TestCartpole]: " + finalCostName + " was not found!");
}
auto createFinalCost = [&]() {
matrix_t Qf(STATE_DIM, STATE_DIM);
loadData::loadEigenMatrix(taskFile, "Q_final", Qf);
Qf *= (timeHorizon / cartPoleInterfacePtr->mpcSettings().timeHorizon_); // 按比例缩放最终状态成本矩阵
return std::make_unique<QuadraticStateCost>(Qf);
};
cartPoleInterfacePtr->optimalControlProblem().finalCostPtr->add(finalCostName, createFinalCost());
// 移除输入限制约束，以便在每个测试用例中重新定义
inputLimitsConstraint = popInequalityLagrangian(inputLimitsName, cartPoleInterfacePtr->optimalControlProblem());
// 设置初始的目标轨迹数据，包括时间、状态和输入轨迹
initTargetTrajectories.timeTrajectory.push_back(0.0);
initTargetTrajectories.stateTrajectory.push_back(cartPoleInterfacePtr->getInitialTarget());
initTargetTrajectories.inputTrajectory.push_back(vector_t::Zero(ocs2::cartpole::INPUT_DIM));
}
// 该成员函数为测试类TestCartpole提供了一个获取GaussNewtonDDP实例的方法，根据GetParam()获取的参数确定具体是SLQ还是ILQR算法
std::unique_ptr<GaussNewtonDDP> TestCartpole::getAlgorithm() const {
// 获取当前测试用例中设置的DDP算法类型
const auto algorithm = std::get<0>(GetParam());
// 获取CartPoleInterface对象中的DDP默认配置，并根据需要进行修改
auto ddpSettings = cartPoleInterfacePtr->ddpSettings();
ddpSettings.algorithm_ = algorithm; // 设置DDP算法类型
ddpSettings.maxNumIterations_ = 100; // 设置最大迭代次数为100次
ddpSettings.minRelCost_ = 1e-12; // 设置最小相对成本改进阈值以避免过早终止迭代
ddpSettings.displayInfo_ = false; // 关闭详细输出信息
ddpSettings.displayShortSummary_ = true; // 显示简短总结信息
// 根据算法类型创建对应的SLQ或ILQR实例
switch (algorithm) {
case ddp::Algorithm::SLQ:
return std::make_unique<SLQ>(std::move(ddpSettings), cartPoleInterfacePtr->getRollout(),
createOptimalControlProblem(PenaltyType::ModifiedRelaxedBarrierPenalty),
cartPoleInterfacePtr->getInitializer());
case ddp::Algorithm::ILQR:
return std::make_unique<ILQR>(std::move(ddpSettings), cartPoleInterfacePtr->getRollout(),
createOptimalControlProblem(PenaltyType::ModifiedRelaxedBarrierPenalty),
cartPoleInterfacePtr->getInitializer());
default:
throw std::runtime_error("[TestCartpole::getAlgorithm] undefined algorithm"); // 如果算法类型未定义，则抛出异常
}
}
// 该成员函数用于根据给定的惩罚类型生成对应的AugmentedPenaltyBase实例
std::unique_ptr<augmented::AugmentedPenaltyBase> TestCartpole::getPenalty() const {
// 获取当前测试用例中设置的惩罚类型
const auto penaltyType = std::get<1>(GetParam());
// 根据惩罚类型创建相应的惩罚策略实例
switch (penaltyType) {
case PenaltyType::SlacknessSquaredHingePenalty: {
using penalty_type = augmented::SlacknessSquaredHingePenalty;
penalty_type::Config boundsConfig;
loadData::loadPenaltyConfig(taskFile, "bounds_penalty_config", boundsConfig, false); // 加载惩罚配置
return penalty_type::create(boundsConfig); // 创建惩罚实例
}
case PenaltyType::ModifiedRelaxedBarrierPenalty: {
using penalty_type = augmented::ModifiedRelaxedBarrierPenalty;
penalty_type::Config boundsConfig;
loadData::loadPenaltyConfig(taskFile, "bounds_penalty_config", boundsConfig, false); // 加载惩罚配置
return penalty_type::create(boundsConfig); // 创建惩罚实例
}
default:
throw std::runtime_error("[TestCartpole::getPenalty] undefined penaltyType"); // 如果惩罚类型未定义，则抛出异常
}
}
// 该成员函数创建一个最优控制问题实例，使用指定的惩罚策略来处理输入限制约束
OptimalControlProblem TestCartpole::createOptimalControlProblem(PenaltyType penaltyType) const {
// 获取原始最优控制问题实例
OptimalControlProblem problem = cartPoleInterfacePtr->getOptimalControlProblem();
// 使用新的惩罚策略重新添加输入限制约束
problem.inequalityLagrangianPtr->add(inputLimitsName,
create(std::unique_ptr<StateInputConstraint>(inputLimitsConstraint->clone()), getPenalty()));
return problem; // 返回带有新惩罚策略的最优控制问题
}
// 该成员函数从最优控制问题中移除并返回指定名称的不等式拉格朗日项（即输入限制约束）
std::unique_ptr<StateInputConstraint> TestCartpole::popInequalityLagrangian(const std::string& name, OptimalControlProblem& ocp) const {
// 从最优控制问题中提取指定名称的不等式拉格朗日项
auto termLagrangianPtr = ocp.inequalityLagrangianPtr->extract(name);
// 检查是否成功找到指定名称的约束
if (termLagrangianPtr == nullptr) {
throw std::runtime_error("[TestCartpole::popInequalityLagrangian]: " + name + " was not found!"); // 若未找到则抛出异常
}
// 确保提取到的约束类型正确，并将其克隆后转换为StateInputConstraint指针
auto termStateInputLagrangianPtr = dynamic_cast<StateInputAugmentedLagrangian*>(termLagrangianPtr.get());
if (termStateInputLagrangianPtr == nullptr) {
throw std::runtime_error("[TestCartpole::popInequalityLagrangian]: term " + name + " is not of type StateInputAugmentedLagrangian!"); // 类型错误时抛出异常
}
return std::unique_ptr<StateInputConstraint>(termStateInputLagrangianPtr->get().clone()); // 返回克隆后的约束实例
}
// testInputLimitsViolation()函数：用于测试输入限制在给定时间轨迹上是否被违反
void TestCartpole::testInputLimitsViolation(const scalar_array_t& timeTrajectory, const std::vector<LagrangianMetricsConstRef>& termMetrics) const {
// 遍历所有时间点上的约束指标
for (size_t i = 0; i < timeTrajectory.size(); i++) {
// 计算当前时间步的输入限制违反量，取负数以表示违反程度（越小表示违反越多）
const vector_t constraintViolation = termMetrics[i].constraint.cwiseMin(0.0);
// 对下限进行检查，预期不违反时其值应接近于0
EXPECT_NEAR(constraintViolation(0), 0.0, constraintViolationTolerance)
<< "Input lower limit is violated at time " + std::to_string(timeTrajectory[i]);
// 上述代码块似乎存在重复，应该是对上限进行检查，但两次都使用了相同的条件判断，这里可能需要修改为针对不同维度或变量的约束检查
// 对上限进行假设性检查，预期不违反时其值也应接近于0
// EXPECT_NEAR(constraintViolation(对应维度索引), 0.0, constraintViolationTolerance)
// << "Input upper limit is violated at time " + std::to_string(timeTrajectory[i]);
}
}
// testFinalState()函数：用于测试求解后的最终状态是否满足期望的目标状态
void TestCartpole::testFinalState(const PrimalSolution& primalSolution) const {
// 获取最优控制问题求解得到的最终状态
const auto& finalState = primalSolution.stateTrajectory_.back();
// 获取目标状态（即初始设定的理想终止状态）
const auto& desiredState = cartPoleInterfacePtr->getInitialTarget();
// 对每个状态分量进行比较，确保它们之间的差距不超过goalViolationTolerance阈值
EXPECT_NEAR(finalState(0), desiredState(0), goalViolationTolerance)
<< "Pole final angle is not as expected (" + std::to_string(desiredState(0)) + ")";
EXPECT_NEAR(finalState(1), desiredState(1), goalViolationTolerance)
<< "Cart final position does not match the target (" + std::to_string(desiredState(1)) + ")";
EXPECT_NEAR(finalState(2), desiredState(2), goalViolationTolerance)
<< "Pole final velocity differs from the desired value (" + std::to_string(desiredState(2)) + ")";
EXPECT_NEAR(finalState(3), desiredState(3), goalViolationTolerance)
<< "Cart final velocity is not as required (" + std::to_string(desiredState(3)) + ")";
}
// 类成员变量定义
const std::string TestCartpole::inputLimitsName = "InputLimits"; // 输入限制项名称
TargetTrajectories TestCartpole::initTargetTrajectories; // 初始目标轨迹数据结构
std::unique_ptr<CartPoleInterface> TestCartpole::cartPoleInterfacePtr; // CartPole系统接口指针
// 私有类成员变量
std::string TestCartpole::taskFile; // 任务文件路径
std::unique_ptr<StateInputConstraint> TestCartpole::inputLimitsConstraint; // 输入限制约束实例
// 类内常量定义
constexpr scalar_t TestCartpole::timeHorizon; // 最大优化时间范围
constexpr scalar_t TestCartpole::goalViolationTolerance; // 目标状态偏离容忍度
constexpr scalar_t TestCartpole::constraintViolationTolerance; // 约束违反容忍度
// Google Test 测试用例实现部分
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
// 单元测试函数：测试DDP算法在不同参数组合下的性能
TEST_P(TestCartpole, testDDP) {
// 构建DDP算法实例
auto ddpPtr = getAlgorithm();
// 设置目标轨迹数据到DDP参考管理器中
ddpPtr->getReferenceManager().setTargetTrajectories(initTargetTrajectories);
// 创建并添加一个观察者模块，用于检测输入限制的违反情况
auto inputLimitsObserverModulePtr = SolverObserver::LagrangianTermObserver(
SolverObserver::Type::Intermediate, "InputLimits", // 在中间迭代阶段检查名为"InputLimits"的拉格朗日项
[&](const scalar_array_t& timeTrajectory, const std::vector<LagrangianMetricsConstRef>& termMetrics) {
testInputLimitsViolation(timeTrajectory, termMetrics); // 调用测试函数来检查是否违反输入限制
});
ddpPtr->addSolverObserver(std::move(inputLimitsObserverModulePtr));
// 执行DDP算法求解过程
ddpPtr->run(0.0, cartPoleInterfacePtr->getInitialState(), timeHorizon);
// 检查求解结果中的最终状态是否满足期望的目标状态
testFinalState(ddpPtr->primalSolution(timeHorizon));
}
// 定义测试用例名称生成函数，以便在gtest输出中显示更友好的测试名称
std::string testName(const testing::TestParamInfo<TestCartpole::ParamType>& info) {
std::string name;
name += ddp::toAlgorithmName(std::get<0>(info.param)) + "_";
switch (std::get<1>(info.param)) {
case PenaltyType::SlacknessSquaredHingePenalty:
name += "SlacknessSquaredHingePenalty";
break;
case PenaltyType::ModifiedRelaxedBarrierPenalty:
name += "ModifiedRelaxedBarrierPenalty";
break;
}
return name;
}
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
// 参数化测试实例化部分，结合不同的DDP算法和惩罚策略进行组合测试
INSTANTIATE_TEST_CASE_P(TestCaseCartpole, TestCartpole,
testing::Combine(testing::ValuesIn({ddp::Algorithm::SLQ, ddp::Algorithm::ILQR}),
testing::ValuesIn({PenaltyType::SlacknessSquaredHingePenalty,
PenaltyType::ModifiedRelaxedBarrierPenalty})),
testName);
该C++代码定义了一个测试类TestCartpole，用于对一个最优控制问题的求解器进行单元测试。测试类基于Google Test框架，针对不同的算法（ILQR或SLQ）和惩罚项类型（SlacknessSquaredHingePenalty或ModifiedRelaxedBarrierPenalty）生成并执行不同的测试用例。
在构造函数中，首先初始化了CartPole问题的相关接口，并根据时间范围调整了最终成本。接着移除了输入限制约束，以便后续根据不同测试参数重新添加。
成员函数getAlgorithm()和getPenalty()分别返回根据测试参数指定的DDP算法实例以及对应的惩罚项实例。
createOptimalControlProblem()函数依据给定的惩罚类型创建一个最优控制问题实例，其中包含了输入限制约束。
testInputLimitsViolation()用于检查输入限制是否被违反，testFinalState()则验证最优轨迹结束时的状态是否接近目标状态。
主要测试方法testDDP()包括以下步骤：
- 根据测试参数创建相应的DDP求解器。
- 设置目标轨迹。
- 添加一个观察者模块，用于在迭代过程中监测输入限制的违反情况。
- 运行求解器以解决最优控制问题。
- 验证最优轨迹终止状态与目标状态的接近程度。
最后，通过INSTANTIATE_TEST_CASE_P宏将所有可能的算法与惩罚项组合成多个测试用例，并为每个测试用例命名。
2.11 CMakeLists.txt
这个CMake文件是为在catkin工作空间中构建ocs2_cartpole包而设置的。它配置了依赖项，包含了必要的包，为clang工具生成编译命令，配置了构建目录，添加了目标库，安装了必需的文件，并定义了单元测试。
# 设置CMake的最小版本要求为3.0.2
cmake_minimum_required(VERSION 3.0.2)
# 定义当前项目名为"ocs2_cartpole"
project(ocs2_cartpole)
# 配置生成编译命令.json文件以支持clang工具链
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
# 定义catkin依赖包列表，包括ocs2_core、ocs2_ddp等几个ocs2系列的库
set(CATKIN_PACKAGE_DEPENDENCIES
ocs2_core
ocs2_ddp
ocs2_mpc
ocs2_robotic_tools
)
# 查找并引入catkin及相关依赖库（Boost库中的system和filesystem组件）
find_package(catkin REQUIRED COMPONENTS ${CATKIN_PACKAGE_DEPENDENCIES})
find_package(Boost REQUIRED COMPONENTS system filesystem)
find_package(Eigen3 3.3 REQUIRED NO_MODULE) # 寻找并引入Eigen3数学库
###################################
## catkin特定配置部分 ##
###################################
# 使用catkin_package()函数定义项目的包含目录、库以及依赖关系
catkin_package(
INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} # 提供给其他catkin包使用的头文件路径
LIBRARIES ${PROJECT_NAME} # 本项目提供的库名
CATKIN_DEPENDS ${CATKIN_PACKAGE_DEPENDENCIES} # 依赖的catkin包
DEPENDS Boost # 非catkin依赖项，这里是Boost库
)
###########
## 构建 ##
###########
# 在编译时动态生成包含项目路径的头文件
configure_file (
"${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in"
"${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY
)
# 添加所有目标的包含目录
include_directories(
include
${EIGEN3_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
# 创建名为"ocs2_cartpole"的接口库
add_library(${PROJECT_NAME}
src/CartPoleInterface.cpp
)
# 设置该库依赖于catkin导出的目标
add_dependencies(${PROJECT_NAME}
${catkin_EXPORTED_TARGETS}
)
# 链接所需的库到"ocs2_cartpole"目标
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
dl # 动态链接库
)
# 设置"ocs2_cartpole"目标的公共编译选项
target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS})
#####################
### Clang Tooling ###
#####################
# 检测并配置Clang工具链（如静态代码分析工具）
find_package(cmake_clang_tools QUIET)
if(cmake_clang_tools_FOUND)
message(STATUS "Running clang tooling.")
add_clang_tooling(
TARGETS ${PROJECT_NAME} # 对"ocs2_cartpole"目标应用工具链
SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test # 指定源代码目录
CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include # 头文件目录
CT_HEADER_EXCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/generated # 排除的头文件目录
CF_WERROR # 错误视为警告
)
endif()
#############
## 安装 ##
#############
# 安装规则：将"ocs2_cartpole"库安装至catkin指定的位置
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# 安装头文件目录
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
# 安装配置文件目录
install(DIRECTORY config
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
#############
## 测试 ##
#############
# 添加名为"test_cartpole"的gtest单元测试
catkin_add_gtest(test_cartpole
test/testCartpole.cpp
)
# 设置单元测试的私有包含目录
target_include_directories(test_cartpole PRIVATE
${PROJECT_BINARY_DIR}/include
)
# 链接必要的库到"test_cartpole"单元测试目标
target_link_libraries(test_cartpole
gtest_main # Google Test主库
${PROJECT_NAME} # "ocs2_cartpole"库本身
${catkin_LIBRARIES} # catkin依赖的库
${Boost_LIBRARIES} # Boost库
)
# 注释说明如何运行单元测试该CMakeLists.txt文件用于在使用catkin工具构建的ROS（机器人操作系统）环境中配置和构建名为"ocs2_cartpole"的项目。主要步骤包括：
-
cmake_minimum_required(VERSION 3.0.2)声明了本项目所需的CMake最低版本为3.0.2。 -
project(ocs2_cartpole)定义了当前CMake项目名称为"ocs2_cartpole"。 -
设置
CMAKE_EXPORT_COMPILE_COMMANDS为ON，以便生成编译命令.json文件以供clang工具使用。 -
定义依赖包列表
CATKIN_PACKAGE_DEPENDENCIES，包含了多个ocs2系列的依赖库。 -
使用
find_package查找并引入catkin以及其它外部依赖库如Boost、Eigen3等。 -
在
catkin_package部分，定义了包含目录、库文件及依赖关系，使得其他项目可以找到并链接到这个项目提供的头文件和库。 -
构建部分：
- 首先通过
configure_file动态生成一个包含项目路径的头文件。 - 然后添加包含目录至全局搜索路径，包括Eigen3、Boost和catkin的相关路径。
- 创建名为"ocs2_cartpole"的库，并指定了源文件以及链接依赖库，包括catkin库、Boost库以及dl（动态链接库）。
- 设定项目的C++编译选项。
- 首先通过
-
Clang Tooling部分：如果找到cmake_clang_tools，则设置相关参数并运行clang工具进行代码静态检查。
-
安装部分：定义了将目标库和头文件安装到相应catkin指定的位置。
-
测试部分：添加名为“test_cartpole”的gtest单元测试，并将其链接到必要的库，包括该项目自身库以及catkin和Boost库。
整个文件涵盖了从依赖管理、编译选项设定、构建目标库、单元测试配置到安装规则等一系列CMake配置过程。
2.12 package.xml
该XML函数是一个用于描述ROS（机器人操作系统）软件包的文件。它包含了软件包的格式、名称、版本、描述、维护者、许可证、构建工具依赖项、构建依赖项和依赖项等信息。这些信息对于安装和管理ROS软件包非常有用。
<?xml version="1.0"?>
<!-- 开始定义一个ROS软件包，格式版本为2 -->
<package format="2">
<!-- 设置软件包名称为"ocs2_cartpole" -->
<name>ocs2_cartpole</name>
<!-- 软件包当前版本号为0.0.0 -->
<version>0.0.0</version>
<!-- 提供软件包的简短描述 -->
<description>The ocs2_cartpole package, related to a cart-pole system in the OCS2 framework.</description>
<!-- 维护者信息，包括姓名和电子邮件地址 -->
<maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer>
<maintainer email="jcarius@ethz.ch">Jan Carius</maintainer>
<maintainer email="rgrandia@ethz.ch">Ruben Grandia</maintainer>
<!-- TODO：此处应填写该软件包遵循的具体开源许可证 -->
<license>TODO</license>
<!-- 构建工具依赖项，本例中使用Catkin作为构建系统 -->
<buildtool_depend>catkin</buildtool_depend>
<!-- 构建阶段依赖的其他ROS软件包 -->
<build_depend>roslib</build_depend>
<build_depend>cmake_clang_tools</build_depend>
<!-- 运行时依赖的其他ROS软件包 -->
<depend>ocs2_core</depend>
<depend>ocs2_ddp</depend>
<depend>ocs2_mpc</depend>
<depend>ocs2_robotic_tools</depend>
</package>该XML文件是一个ROS（Robot Operating System，机器人操作系统）的package.xml配置文件，详细信息如下：
-
<?xml version="1.0"?>：声明这是一个XML文档，版本为1.0。 -
<package format="2">：定义一个软件包，格式版本为2。 -
<name>ocs2_cartpole</name>：指定软件包名称为"ocs2_cartpole"。 -
<version>0.0.0</version>：设置软件包版本号为0.0.0。 -
<description>The ocs2_cartpole package</description>：提供软件包的简要描述。 -
<maintainer>标签块：列出三位维护者及其联系方式，分别是Farbod Farshidian、Jan Carius和Ruben Grandia。 -
<license>TODO</license>：此处尚未填写软件包的许可证信息。 -
<buildtool_depend>catkin</buildtool_depend>：表明此软件包在构建时依赖于Catkin构建系统。 -
<build_depend>标签块：列举了在构建阶段需要的依赖包，如roslib和cmake_clang_tools。 -
<depend>标签块：列举了在运行阶段需要的依赖包，包括ocs2_core、ocs2_ddp、ocs2_mpc和ocs2_robotic_tools。
总结起来，这个XML文件定义了一个名为"ocs2_cartpole"的ROS软件包，包含了版本、描述、维护者、许可证以及构建和运行所需的依赖项等信息。
三、ocs2_cartpole文件夹
3.1 include\ocs2_cartpole_ros\CartpoleDummyVisualization.h
这段代码定义了一个名为CartpoleDummyVisualization的类，该类继承自DummyObserver类。它具有一个构造函数和一个析构函数，以及一个重写的方法update。它还具有一个私有方法launchVisualizerNode和一个私有成员变量jointPublisher_。
/******************************************************************************
Copyright (c) 2017, Farbod Farshidian. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/
#pragma once
// 包含必要的头文件，如ROS库、JointState消息类型等
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
// 引入自定义的DummyObserver类
#include <ocs2_ros_interfaces/mrt/DummyObserver.h>
// 引入cartpole系统相关的定义
#include "ocs2_cartpole/definitions.h"
namespace ocs2 {
namespace cartpole {
/**
* CartpoleDummyVisualization类：继承自DummyObserver类，用于在ROS环境中对cartpole系统的状态进行可视化展示
*/
class CartpoleDummyVisualization : public DummyObserver {
public:
/**
* 构造函数：通过给定的ROS节点句柄初始化并启动可视化节点
* @param nodeHandle ROS节点句柄
*/
explicit CartpoleDummyVisualization(ros::NodeHandle& nodeHandle) { launchVisualizerNode(nodeHandle); }
/**
* 默认析构函数
*/
~CartpoleDummyVisualization() override = default;
/**
* 重载更新方法：当cartpole系统状态、最优策略或控制命令更新时，此方法被调用以发布新数据到ROS进行可视化
* @param observation 系统观测信息
* @param policy 最优策略
* @param command 控制命令
*/
void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override;
private:
/**
* 私有方法：创建和初始化一个ROS节点，用于发布cartpole关节状态信息
* @param nodeHandle ROS节点句柄
*/
void launchVisualizerNode(ros::NodeHandle& nodeHandle);
/**
* 私有成员变量：用于发布cartpole关节状态信息的ROS发布器
*/
ros::Publisher jointPublisher_;
};
} // namespace cartpole
} // namespace ocs2
// 这个类主要用于接收cartpole系统的信息，并将其转换为ROS消息格式，通过jointPublisher_发布出去，进而实现cartpole系统的状态在ROS环境中的可视化。该类CartpoleDummyVisualization主要用于在ROS（Robot Operating System）环境中实现对cartpole系统的可视化。它继承自ocs2::DummyObserver基类，意味着它是一个观察者对象，可以接收并处理系统状态、策略和命令数据。
构造函数：
explicit CartpoleDummyVisualization(ros::NodeHandle& nodeHandle)通过传入一个ROS节点句柄初始化，并调用私有方法launchVisualizerNode来启动一个用于可视化的ROS节点。
析构函数：
~CartpoleDummyVisualization() override = default;默认析构函数，确保资源正确释放。
重写的方法：
void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) override;当系统状态、最优策略或控制命令更新时，此方法会被调用，以便将新数据发送到ROS进行可视化展示。
私有方法：
void launchVisualizerNode(ros::NodeHandle& nodeHandle);这个方法负责创建并初始化一个ROS节点，用于发布cartpole系统的关节状态信息。
私有成员变量：
ros::Publisher jointPublisher_;这是一个ROS发布器，用于发布sensor_msgs::JointState类型的消息，将cartpole的关节状态信息发送给ROS中的其他节点，如rviz等可视化工具。
3.2 launch\cartpole.launch
这段XML代码是一个ROS（机器人操作系统）的launch文件，用于启动ocs2_cartpole_ros包中的cartpole_mpc和cartpole_dummy_test节点。通过设置参数rviz和multiplot的值，条件性地包括相应的launch文件。最后，通过指定任务名称参数，启动cartpole_mpc节点。
<!-- 定义一个ROS launch文件，用于组织和控制ocs2_cartpole_ros包中的节点启动过程 -->
<launch> <!-- 标记开始一个launch文件 -->
<!-- 声明3个命令行参数 -->
<arg name="rviz" default="true" /> <!-- 参数rviz，默认值为true，决定是否启用可视化功能 -->
<arg name="multiplot" default="true"/> <!-- 参数multiplot，默认值为true，决定是否启动多图展示功能 -->
<arg name="task_name" default="mpc"/> <!-- 参数task_name，默认值为"mpc"，作为任务名称传递给后续节点 -->
<!-- 根据参数rviz的值有条件地包含并执行相应的launch文件 -->
<group if="$(arg rviz)"> <!-- 如果rviz参数为真 -->
<include file="$(find ocs2_cartpole_ros)/launch/visualize.launch"/> <!-- 包含并执行ocs2_cartpole_ros包下的visualize.launch文件 -->
</group>
<!-- 同样根据参数multiplot的值有条件地执行另一个launch文件 -->
<group if="$(arg multiplot)"> <!-- 如果multiplot参数为真 -->
<include file="$(find ocs2_cartpole_ros)/launch/multiplot.launch" /> <!-- 包含并执行ocs2_cartpole_ros包下的multiplot.launch文件 -->
</group>
<!-- 启动名为cartpole_mpc的ROS节点 -->
<node pkg="ocs2_cartpole_ros" type="cartpole_mpc" name="cartpole_mpc"> <!-- 指定节点所在包、类型和名称 -->
<!-- 输出重定向到屏幕 -->
<param name="output" value="screen"/>
<!-- 传入之前定义的任务名称参数 -->
<param name="args" value="$(arg task_name)"/>
<!-- 不设置特殊的启动前缀 -->
<param name="launch-prefix" value=""/>
</node>
<!-- 启动名为cartpole_dummy_test的ROS节点 -->
<node pkg="ocs2_cartpole_ros" type="cartpole_dummy_test" name="cartpole_dummy_test">
<!-- 输出同样重定向到屏幕 -->
<param name="output" value="screen"/>
<!-- 传入之前定义的任务名称参数 -->
<param name="args" value="$(arg task_name)"/>
<!-- 设置启动前缀，在新的GNOME终端窗口中启动该节点 -->
<param name="launch-prefix" value="gnome-terminal --"/>
</node>
</launch> <!-- 结束launch文件标记 -->这段XML代码定义了一个ROS（机器人操作系统）的启动配置文件，用于组织和控制ocs2_cartpole_ros包中相关节点的启动过程。具体解析如下：
-
首先声明了3个命令行参数：
rviz：默认值为"true"，决定是否加载可视化相关的配置。multiplot：同样默认值为"true"，判断是否需要启动多图展示功能。task_name：默认值为"mpc"，这是传递给后续节点作为运行时参数的任务名称。
-
<group if="$(arg rviz)">和<group if="$(arg multiplot)">标签分别表示根据参数rviz与multiplot的值执行相应操作。如果rviz参数为真，则会包含并执行visualize.launch文件；若multiplot参数为真，则会执行multiplot.launch文件。这两个launch文件都位于ocs2_cartpole_ros包下。 -
<node>标签用于启动具体的ROS节点：- 第一个节点是
cartpole_mpc，输出重定向到屏幕，并且接收通过args属性传入的$(arg task_name)作为其运行参数，即任务名称，默认为"mpc"。 - 第二个节点是
cartpole_dummy_test，同样输出重定向到屏幕，它也接收任务名称作为参数，但它的启动前缀设置为“gnome-terminal --”，这意味着该节点将在一个新的GNOME终端窗口中启动。
- 第一个节点是
总结来说，这段XML脚本主要负责管理和协调多个ROS节点的启动顺序和条件，并根据用户提供的命令行参数进行相应的调整和配置。
3.3 launch\multiplot.launch
这段XML代码是一个ROS（机器人操作系统）launch文件，用于启动RQT Multi-plot工具并生成多个可视化图表。它定义了两个参数observation_config和metrics_config，分别指定观察配置和指标配置的文件路径。然后，它使用这些参数作为输入，分别启动了两个节点mpc_observation和mpc_metrics，并将它们与RQT Multi-plot工具关联。最后，它通过include标签包含了一个其他launch文件，用于生成性能指标的可视化图表。
<!-- 定义launch文件中的参数 -->
<arg name="observation_config" default="$(find ocs2_cartpole)/config/multiplot/mpc_observation.xml" />
<arg name="metrics_config" default="$(find ocs2_cartpole)/config/multiplot/mpc_metrics.xml" />
<!-- 注释：设置两个参数，分别指向用于RQT Multi-plot的观测数据配置文件和性能指标配置文件 -->
<!-- 启动第一个RQT Multi-plot节点以显示观测数据 -->
<node name="mpc_observation" pkg="rqt_multiplot" type="rqt_multiplot"
args="--multiplot-run-all --multiplot-config $(arg observation_config)"
output="screen"/>
<!-- 注释：使用`rqt_multiplot`包中的节点类型启动名为"mpc_observation"的节点，加载指定的观测数据配置文件，并实时输出至屏幕 -->
<!-- 启动第二个RQT Multi-plot节点以显示性能指标 -->
<node name="mpc_metrics" pkg="rqt_multiplot" type="rqt_multiplot"
args="--multiplot-run-all --multiplot-config $(arg metrics_config)"
output="screen"/>
<!-- 注释：与上一个节点类似，但加载的是性能指标配置文件，同样实时输出至屏幕 -->
<!-- 包含并启动另一个launch文件以展示性能指数图表 -->
<include file="$(find ocs2_ros_interfaces)/launch/performance_indices.launch">
<arg name="mpc_policy_topic_name" value="cartpole_mpc_policy"/>
</include>
<!-- 注释：通过include标签引入并执行"ocs2_ros_interfaces"包下的"performance_indices.launch"文件，传入自定义参数mpc_policy_topic_name，值为"cartpole_mpc_policy"，该参数可能用于在性能指数图表中指定特定的MPC策略主题名称 -->该XML代码片段是一个用于ROS（Robot Operating System）系统的launch文件，主要功能是启动并配置RQT Multi-plot工具以展示不同数据的图表。具体解析如下：
-
<arg>标签定义了两个参数：observation_config的默认值指向 "ocs2_cartpole" 包下的 "config/multiplot/mpc_observation.xml" 文件，这个文件可能包含了观测数据的可视化配置信息。metrics_config的默认值指向的是与上述类似路径的 "mpc_metrics.xml" 文件，这个文件则可能包含了性能指标相关的可视化配置。
-
接下来的两个
<node>标签分别启动了两个RQT Multi-plot实例：- 第一个节点名为 "mpc_observation"，通过
args属性传递参数--multiplot-run-all --multiplot-config $(arg observation_config)，表示运行所有预设的绘图任务，并使用之前定义的observation_config参数作为配置文件来绘制观察数据图表。 - 第二个节点 "mpc_metrics" 同样原理，但使用
metrics_config参数配置，用于显示性能指标相关的图表。
- 第一个节点名为 "mpc_observation"，通过
-
<include>标签引入了另一个launch文件"$(find ocs2_ros_interfaces)/launch/performance_indices.launch"，并且传入了一个自定义参数mpc_policy_topic_name，其值为 "cartpole_mpc_policy"。这个被包含的launch文件可能会设置和启动额外的节点或插件，用来收集和展示关于MPC策略性能指数的相关数据。
总结来说，这段XML代码用于启动和配置多个RQT Multi-plot窗口，以便实时监控和分析“ocs2_cartpole”应用中的观测数据和性能指标。
3.4 launch\visualize.launch
这段XML代码是一个ROS（机器人操作系统）launch文件，用于启动一个包含机器人状态发布器和RViz可视化工具的ROS节点。其中定义了参数和节点，包括rviz参数、rviz配置文件、机器人模型等，并使用条件判断来决定是否启动RViz。
<!-- 启动文件开始，用于部署ROS相关节点和参数 -->
<launch> <!-- ROS launch文件标签，定义启动过程 -->
<!-- 定义命令行参数 -->
<arg name="rviz" default="true" />
<!-- 参数 "rviz" 默认值为 true，表示是否启用 RViz 可视化工具 -->
<arg name="rvizconfig" default="$(find ocs2_cartpole_ros)/rviz/cartpole.rviz" />
<!-- 参数 "rvizconfig" 指定了 RViz 的默认配置文件路径，通过 $(find) 功能查找包中的资源文件 -->
<arg name="model" default="$(find ocs2_robotic_assets)/resources/cartpole/urdf/cartpole.urdf"/>
<!-- 参数 "model" 指定了机器人URDF模型的默认路径，同样使用 $(find) 查找指定包中的URDF模型 -->
<!-- 设置全局ROS参数 -->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
<!-- 将 "robot_description" 参数设置为执行命令的结果，该命令会调用 xacro 工具解析并展开指定的URDF模型文件 -->
<!-- 启动ROS节点：机器人状态发布器 -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- 该节点负责发布机器人的TF（变换）消息，描述机器人的位姿信息 -->
<!-- 条件性启动ROS节点：RViz可视化工具 -->
<node pkg="rviz" type="rviz" name="Cartpole" args="-d $(arg rvizconfig)" output="screen" if="$(arg rviz)"/>
<!-- 根据 "rviz" 参数判断是否启动 RViz 节点，若为 true，则加载指定的 RViz 配置文件，并将输出重定向到屏幕显示 -->
</launch>
<!-- 启动文件结束 -->这段XML代码是一个ROS（机器人操作系统）的launch文件配置，用于部署和启动相关节点以运行一个名为cartpole的机器人系统。具体解析如下：
-
定义三个命令行参数：
rviz：布尔型参数，默认值为"true"，决定是否启动RViz可视化工具。rvizconfig：定义了RViz的配置文件路径，默认指向ocs2_cartpole_ros包中的cartpole.rviz文件。model：指定了机器人的URDF模型文件路径，默认指向ocs2_robotic_assets包中cartpole.urdf文件。
-
使用
<param>标签设置了一个全局ROS参数：- 名称为"robot_description"，其值通过执行命令
$(find xacro)/xacro --inorder $(arg model)获得。该命令会找到并调用xacro工具来解析并展开指定的URDF模型文件。
- 名称为"robot_description"，其值通过执行命令
-
启动两个ROS节点：
<node>标签定义了名为"robot_state_publisher"的节点，来自"robot_state_publisher"包，类型为"robot_state_publisher"，这个节点负责发布机器人的TF（变换）消息，从而在系统中描述机器人的位姿信息。
-
第二个
<node>标签也定义了一个ROS节点：- 来自"rviz"包，类型为"rviz"，节点名称为"Cartpole"，根据之前定义的rviz参数，使用"-d"选项加载指定的RViz配置文件。
- 这个节点只有在
rviz参数为"true"时才会启动（通过if="$(arg rviz)"判断），并且其输出重定向到屏幕显示。
总之，这段launch文件主要目的是初始化机器人模型参数、启动状态发布器，并按需开启RViz进行可视化展示。
3.5 rviz\cartpole.rviz
该yaml文件是一个包含多个面板的配置文件。不同面板有不同的属性和设置，如可视化管理器、工具栏、窗口几何等。每个面板都有自己的配置选项，如树的高度、面板的展开状态、颜色等。这个配置文件可以用来自定义 rviz 的外观和布局。
# Rviz配置文件，用于定义Rviz的布局、显示项、工具栏及窗口几何信息
Panels: # 定义一系列面板及其属性
- Class: rviz/Displays # 类型为“显示”面板
Help Height: 138 # 面板内帮助区域的高度
Name: Displays # 面板名称为"Displays"
Property Tree Widget: # 属性树小部件设置
Expanded: # 列出启动时默认展开的节点路径列表
- /Global Options1
- /Status1
- /Grid1
- /Grid1/Offset1
Splitter Ratio: 0.5 # 分割器比例，决定属性树和其余内容的空间分配
Tree Height: 270 # 属性树区域高度
# 后续还有Selection, Tool Properties, Views 和 Time面板的详细配置
- Class: rviz/Selection # 类型为“选择”面板
Name: Selection # 面板名称为"Selection"
- Class: rviz/Tool Properties # 类型为“工具属性”面板
Expanded: # 默认展开的节点路径列表
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties # 面板名称为"Tool Properties"
Splitter Ratio: 0.5886790156364441 # 分割器比例
- Class: rviz/Views # 类型为“视图”面板
Expanded: # 默认展开的节点路径列表
- /Current View1
Name: Views # 面板名称为"Views"
Splitter Ratio: 0.5 # 分割器比例
- Class: rviz/Time # 类型为“时间”面板
Experimental: false # 是否为实验性功能
Name: Time # 面板名称为"Time"
SyncMode: 0 # 时间同步模式
SyncSource: "" # 时间同步源
Preferences: # 全局偏好设置
PromptSaveOnExit: true # 退出时提示保存当前配置
Toolbars: # 工具栏相关设置
toolButtonStyle: 2 # 工具按钮样式
Visualization Manager: # 可视化管理器配置
Class: "" # 空字符串，通常包含具体的可视化组件类名
Displays: # 显示组件列表，包括网格(Grid)和机器人模型(RobotModel)
- Alpha: 0.5 # 透明度
Class: rviz/Grid
# ... 更多Grid组件的详细配置参数
- Alpha: 1 # 透明度
Class: rviz/RobotModel
# ... 更多RobotModel组件的详细配置参数
Enabled: true # 可视化管理器启用状态
Global Options: # 全局选项
Background Color: 255; 255; 255 # 背景颜色
# ... 更多全局选项如固定帧、帧率等
Tools: # 工具列表，例如交互、移动相机、选择等工具及其参数
Views: # 视图相关设置
Value: true # 表示该可视化管理器有效
Window Geometry: # 窗口几何布局信息
Displays: # “显示”面板的折叠状态和其他属性
collapsed: true
Selection: # “选择”面板的折叠状态
collapsed: false
Time: # “时间”面板的折叠状态
collapsed: false
Tool Properties: # “工具属性”面板的折叠状态
collapsed: false
Views: # “视图”面板的折叠状态
collapsed: true
Width: 1393 # 窗口宽度
Height: 564 # 窗口高度
X: 2447 # 窗口左上角X坐标
Y: 55 # 窗口左上角Y坐标
QMainWindow State: ... # QMainWindow窗口的状态信息（存储窗口分割器位置等）
# 上述配置信息确保了当Rviz重新加载此配置文件时，能够恢复到先前定制的界面布局、显示项配置以及工具栏和窗口尺寸。该yaml文件是用于配置Robot Operating System (ROS)中Rviz视觉化工具的参数文件。详细解析如下：
-
Panels：
- 定义了5个面板，包括"Displays"、"Selection"、"Tool Properties"、"Views"和"Time"。
- 每个面板都有
Class属性来指定其类型。 Help Height、Tree Height定义了显示帮助信息的高度和树状结构的高度。Expanded列表指定了在属性树中默认展开的节点路径。Splitter Ratio表示面板内部各部分的分割比例。
- 每个面板都有
- 定义了5个面板，包括"Displays"、"Selection"、"Tool Properties"、"Views"和"Time"。
-
Preferences：
PromptSaveOnExit设置为true，这意味着在退出Rviz时会提示用户保存当前配置。
-
Visualization Manager：
- 包含多个显示项（displays），如Grid和RobotModel等，并对它们进行详细的配置，例如颜色、透明度、大小、更新频率以及所关联的数据源等。
-
Global Options：
- 设置了背景颜色、固定帧、帧率等全局参数。
-
Tools：
- 列出了Rviz中启用的一系列工具，如交互、移动相机、选择、聚焦相机、测量、设置初始位姿等，并为每个工具设置了相应的参数。
-
Window Geometry：
- 记录了窗口及其各个面板的状态，如宽度、高度、位置（X, Y坐标）、是否折叠（collapsed）等布局信息。
总结来说，这个yaml文件全面地描述了Rviz的界面布局、可视化元素的配置以及工具选项等细节，以便在重新加载配置时恢复到先前设定的状态。
3.6 src\CartpoleDummyVisualization.cpp
这段代码定义了一个名为CartpoleDummyVisualization的类，其中包括了两个函数update和launchVisualizerNode。update函数根据输入的观测数据和控制策略，生成一个joint_state消息并发布到ROS节点的一个话题上。launchVisualizerNode函数创建并初始化一个jointPublisher_对象，用于发布joint_state消息。这段代码还包含了一个头文件和一个命名空间。
/*******************************************************************************
这段代码定义了一个名为CartpoleDummyVisualization的类，
该类主要用于处理cartpole系统的状态观测数据，
并将其转换为ROS标准的消息类型sensor_msgs::JointState进行发布，
以供其他ROS节点（如可视化工具）订阅并显示cartpole系统的实时运动状态。
通过launchVisualizerNode方法启动一个ROS节点并建立发布者，
而update方法则是周期性地获取系统状态并更新发布的关节状态信息。
*******************************************************************************/
#include "ocs2_cartpole_ros/CartpoleDummyVisualization.h"
// 定义命名空间
namespace ocs2 {
namespace cartpole {
class CartpoleDummyVisualization {
public:
// 更新函数，根据观测到的状态信息和策略计算关节状态并发布
void update(const SystemObservation& observation, const PrimalSolution& policy, const CommandData& command) {
// 创建一个ROS JointState消息对象
sensor_msgs::JointState joint_state;
// 设置消息的时间戳为当前时间
joint_state.header.stamp = ros::Time::now();
// 设置关节名称和位置数组大小为2
joint_state.name.resize(2);
joint_state.position.resize(2);
// 填充关节名称
joint_state.name[0] = "slider_to_cart";
joint_state.name[1] = "cart_to_pole";
// 从系统观测状态中提取关节位置信息
joint_state.position[0] = observation.state(1); // 可能是滑块至小车的位置
joint_state.position[1] = observation.state(0); // 可能是小车至杆的位置
// 将关节状态信息发布到主题"joint_states"
jointPublisher_.publish(joint_state);
}
// 启动可视化节点并发布关节状态话题
void launchVisualizerNode(ros::NodeHandle& nodeHandle) {
// 在给定的ROS节点句柄上创建并初始化一个用于发布关节状态的发布者
jointPublisher_ = nodeHandle.advertise<sensor_msgs::JointState>("joint_states", 1); // 发布频率QoS设置为1
}
private:
// ROS关节状态发布者
ros::Publisher jointPublisher_;
};
} // namespace cartpole
} // namespace ocs2该代码片段属于一个名为CartpoleDummyVisualization的C++类，该类用于在ROS（Robot Operating System）环境中对cartpole系统的状态进行可视化。它属于"ocs2"和"cartpole"命名空间。
-
update函数：- 输入参数：
SystemObservation& observation：表示cartpole系统的当前观测状态。const PrimalSolution& policy：表示cartpole系统当前采用的最优控制策略或决策。const CommandData& command：表示对系统的命令数据，这里未被使用。
- 函数功能：根据观测到的状态信息构建一个
sensor_msgs::JointState类型的ROS消息，其中包含了cartpole系统的两个关节（slider_to_cart 和 cart_to_pole）的位置信息，并发布到名为"joint_states"的主题上。
- 输入参数：
-
launchVisualizerNode函数：- 输入参数：
ros::NodeHandle& nodeHandle—— ROS中的节点句柄，用于与ROS系统交互。 - 函数功能：利用给定的节点句柄创建并初始化一个发布者（publisher），用于发布关节状态信息至"joint_states"主题，该主题通常会被机器人模拟器（如Gazebo）或可视化工具（如rviz）订阅，以实现系统的动态可视化展示。
- 输入参数：
总结来说，这个类主要用于将cartpole系统的状态信息转换为ROS格式并发布出去，以便在支持ROS的可视化工具中显示cartpole模型的运动状态。
3.7 src\CartpoleMpcNode.cpp
这段代码是一个C++程序，主要功能是通过ROS（机器人操作系统）启动一个MPC（模型预测控制）节点，实现对"cartpole"机器人的控制。程序从命令行参数中获取任务文件夹名称，然后初始化ROS节点。接着，它使用任务文件和一些设置创建了一个CartPoleInterface对象，并用该对象初始化了GaussNewtonDDP_MPC对象。程序还创建了一个状态-输入边界观察器，并将其添加到MPC的求解器中。最后，程序通过MPC_ROS_Interface对象启动了相关的ROS节点。
/**************************************************************************
此程序的主要目的是基于给定的任务配置文件初始化一个针对cartpole机器人的模型预测控制器（MPC），
并通过ROS将其部署至机器人控制系统中。
**************************************************************************/
// 此部分包含版权信息和许可协议，表明代码的使用条件和免责声明。
#include <memory> // 引入智能指针库
#include <ros/init.h> // 引入ROS初始化相关头文件
#include <ros/package.h> // 引入获取ROS包路径的头文件
// 引入项目特定的头文件
#include <ocs2_cartpole/CartPoleInterface.h> // CartPole系统的接口类
#include <ocs2_ddp/GaussNewtonDDP_MPC.h> // 基于Gauss-Newton DDP的MPC实现
#include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h> // ROS与MPC交互接口
#include <ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h> // 用于观察器回调函数的ROS接口
int main(int argc, char** argv) {
const std::string robotName = "cartpole"; // 设置机器人的名称为"cartpole"
// 获取任务文件夹参数
std::vector<std::string> programArgs{};
::ros::removeROSArgs(argc, argv, programArgs); // 移除ROS特定参数
if (programArgs.size() <= 1) { // 检查是否有指定的任务文件夹名
throw std::runtime_error("No task file specified. Aborting."); // 若未指定，则抛出异常并退出程序
}
std::string taskFileFolderName(programArgs[1]); // 获取第一个参数作为任务文件夹名
// 初始化ROS节点
ros::init(argc, argv, robotName + "_mpc"); // 初始化ROS节点，节点名为"cartpole_mpc"
ros::NodeHandle nodeHandle; // 创建一个ROS节点句柄
// 创建CartPole系统接口对象
const std::string taskFile = ros::package::getPath("ocs2_cartpole") + "/config/" + taskFileFolderName + "/task.info";
const std::string libFolder = ros::package::getPath("ocs2_cartpole") + "/auto_generated";
ocs2::cartpole::CartPoleInterface cartPoleInterface(taskFile, libFolder, true /*verbose*/);
// 根据ROS包路径和给定的任务文件夹构造任务文件路径，并创建CartPole接口实例，开启详细输出模式
// 创建并配置MPC求解器
ocs2::GaussNewtonDDP_MPC mpc(cartPoleInterface.mpcSettings(),
cartPoleInterface.ddpSettings(),
cartPoleInterface.getRollout(),
cartPoleInterface.getOptimalControlProblem(),
cartPoleInterface.getInitializer());
// 使用CartPole接口提供的各项设置和功能来构建MPC求解器
// 创建并添加观察器以监测输入限制约束
auto createStateInputBoundsObserver = [&]() {
const std::string observingLagrangianTerm = "InputLimits";
const ocs2::scalar_array_t observingTimePoints{0.0, 0.5};
std::vector<std::string> metricsTopicNames;
std::vector<std::string> multiplierTopicNames;
for (const auto& t : observingTimePoints) {
const int timeMs = static_cast<int>(t * 1000.0);
metricsTopicNames.push_back("metrics/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead");
multiplierTopicNames.push_back("multipliers/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead");
}
auto lagrangianCallback = ocs2::ros::createLagrangianCallback(nodeHandle, observingTimePoints, metricsTopicNames,
ocs2::ros::CallbackInterpolationStrategy::linear_interpolation);
auto multiplierCallback = ocs2::ros::createMultiplierCallback(nodeHandle, observingTimePoints, multiplierTopicNames,
ocs2::ros::CallbackInterpolationStrategy::linear_interpolation);
return ocs2::SolverObserver::LagrangianTermObserver(ocs2::SolverObserver::Type::Intermediate, observingLagrangianTerm,
std::move(lagrangianCallback), std::move(multiplierCallback));
};
mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver());
// 定义一个Lambda函数生成观察器，该观察器在指定时间点观察输入约束对应的拉格朗日项和乘子，并将这些数据发布到相应的ROS主题上
// 启动MPC ROS节点
ocs2::MPC_ROS_Interface mpcNode(mpc, robotName);
mpcNode.launchNodes(nodeHandle);
// 使用MPC实例和机器人名称创建MPC_ROS_Interface对象，并启动相关的ROS节点和消息循环
return 0; // 程序正常结束
}该C++程序是一个基于ROS（Robot Operating System）的模型预测控制器（MPC）应用，主要针对一个名为“cartpole”的机器人系统。程序执行以下步骤：
-
参数获取与验证：从命令行参数中提取任务文件所在的文件夹名。如果没有提供，则抛出运行时错误。
-
ROS节点初始化：使用ROS库初始化一个ROS节点，名称为
cartpole_mpc。 -
创建机器人接口：
- 根据提供的任务文件夹路径，构建任务文件和自动生成代码所在的库文件夹路径。
- 使用这些路径创建一个
ocs2::cartpole::CartPoleInterface对象，用于描述和模拟cartpole机器人的动力学特性、成本函数和约束条件，并开启详细输出模式。
-
配置并初始化MPC求解器：
- 使用
CartPoleInterface对象中的设置和相关方法构造一个ocs2::GaussNewtonDDP_MPC对象，这是实现MPC算法的核心部分。
- 使用
-
添加观察器以监测输入限制：
- 定义一个回调函数，该函数在给定的时间点观测输入限制对应的拉格朗日项及其乘子，并将这些信息发布到ROS主题上，以便于监控和调试。
-
将观察器添加到MPC求解器：将上述定义的观察器添加到MPC求解器中。
-
启动MPC ROS节点：
- 创建一个
ocs2::MPC_ROS_Interface对象，该对象将MPC实例与ROS框架连接起来。 - 调用
launchNodes方法启动ROS相关的节点和发布/订阅的消息循环，使得MPC可以实时接收传感器数据并发布控制指令。
- 创建一个
通过以上步骤，该程序实现了对cartpole机器人的MPC控制器设计与部署，并利用ROS进行通信与交互。
3.8 src\DummyCartpoleNode.cpp
这段C++代码是一个ROS节点，用于控制一个名为cartpole的机器人系统。它从命令行参数中获取任务文件的路径，然后初始化ROS节点和机器人接口。接下来，它使用MRT_ROS_Interface类来处理MRT（Model Reference Tracking，模型参考跟踪）并启动一些ROS节点。然后，它创建一个虚拟可视化对象，并使用MRT_ROS_Dummy_Loop类来运行一个虚拟循环，该循环会根据机器人的初始状态和目标轨迹进行控制。最后，该程序返回0表示成功运行。
/******************************************************************************
Copyright (c) 2017, Farbod Farshidian. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/
// 引入ROS相关头文件和自定义库文件
#include <ros/init.h>
#include <ros/package.h>
#include <ocs2_ros_interfaces/mrt/MRT_ROS_Dummy_Loop.h>
#include <ocs2_ros_interfaces/mrt/MRT_ROS_Interface.h>
#include <ocs2_cartpole/CartPoleInterface.h>
#include <ocs2_cartpole/definitions.h>
#include "ocs2_cartpole_ros/CartpoleDummyVisualization.h"
int main(int argc, char** argv) {
// 定义机器人的名称为"cartpole"
const std::string robotName = "cartpole";
// 获取命令行参数中的任务文件夹名
std::vector<std::string> programArgs{};
::ros::removeROSArgs(argc, argv, programArgs);
if (programArgs.size() <= 1) {
throw std::runtime_error("No task file specified. Aborting.");
}
std::string taskFileFolderName = std::string(programArgs[1]);
// 初始化ROS节点
ros::init(argc, argv, robotName + "_mrt");
ros::NodeHandle nodeHandle;
// 创建机器人接口实例，加载任务信息并连接到自动生成的库文件
const std::string taskFile = ros::package::getPath("ocs2_cartpole") + "/config/" + taskFileFolderName + "/task.info";
const std::string libFolder = ros::package::getPath("ocs2_cartpole") + "/auto_generated";
ocs2::cartpole::CartPoleInterface cartPoleInterface(taskFile, libFolder, false /*verbose*/);
// 初始化MRT ROS接口，用于（Model Reference Tracking，模型参考跟踪）
ocs2::MRT_ROS_Interface mrt(robotName);
mrt.initRollout(&cartPoleInterface.getRollout()); // 初始化滚动过程模型
mrt.launchNodes(nodeHandle); // 启动与MRT相关的其他ROS节点
// 创建虚拟可视化对象
auto cartpoleDummyVisualization = std::make_shared<ocs2::cartpole::CartpoleDummyVisualization>(nodeHandle);
// 创建并配置dummy loop（基于MRT）
ocs2::MRT_ROS_Dummy_Loop dummyCartpole(mrt,
cartPoleInterface.mpcSettings().mrtDesiredFrequency_, // MRT循环频率
cartPoleInterface.mpcSettings().mpcDesiredFrequency_); // MPC循环频率
dummyCartpole.subscribeObservers({cartpoleDummyVisualization}); // 订阅可视化观察者
// 设置初始状态
ocs2::SystemObservation initObservation;
initObservation.state = cartPoleInterface.getInitialState(); // 获取机器人的初始状态
initObservation.input.setZero(ocs2::cartpole::INPUT_DIM); // 输入初始化为零向量
initObservation.time = 0.0; // 初始时间设为0.0
// 设置初始目标轨迹
const ocs2::TargetTrajectories initTargetTrajectories(
{0.0}, // 起始时间
{cartPoleInterface.getInitialTarget()}, // 初始状态目标
{ocs2::vector_t::Zero(ocs2::cartpole::INPUT_DIM)} // 初始输入目标
);
// 运行dummy loop，根据初始状态和目标轨迹进行控制，直到ROS节点运行结束
dummyCartpole.run(initObservation, initTargetTrajectories);
// 程序成功执行，返回0
return 0;
}该C++代码是一个主函数，用于启动一个基于ROS（Robot Operating System）的节点来控制名为"cartpole"的机器人系统。主要功能如下：
-
参数解析：从命令行参数中获取任务文件所在的文件夹名，若未提供则抛出错误。
-
ROS初始化：调用
ros::init()函数初始化ROS节点，并命名为“cartpole_mrt”。 -
创建机器人接口：
- 根据ROS包路径构造出task.info文件的实际路径和自动生成库文件夹的路径。
- 使用
ocs2::cartpole::CartPoleInterface类实例化机器人接口，加载任务信息并连接到对应的库文件。
-
MRT初始化：
- 实例化
ocs2::MRT_ROS_Interface对象mrt，它负责处理（Model Reference Tracking，模型参考跟踪）。 - 调用
mrt.initRollout()方法初始化滚动过程，并通过mrt.launchNodes()启动与MRT相关的其他ROS节点。
- 实例化
-
可视化：
- 创建一个
ocs2::cartpole::CartpoleDummyVisualization对象，用于虚拟地展示cartpole系统的状态或行为。
- 创建一个
-
创建并配置dummy loop：
- 初始化
ocs2::MRT_ROS_Dummy_Loop对象，设置循环频率，并订阅了上述的可视化观察者。
- 初始化
-
设定初始状态和目标轨迹：
- 从机器人接口获取初始状态和目标，并包装成
ocs2::SystemObservation和ocs2::TargetTrajectories对象。
- 从机器人接口获取初始状态和目标，并包装成
-
运行dummy loop：
- 将初始状态和目标轨迹传给
dummyCartpole.run()函数，开始执行循环控制逻辑，直至ROS节点正常关闭。
- 将初始状态和目标轨迹传给
-
程序返回：最后，函数返回0表示程序已成功执行完毕。
3.9 CMakeLists.txt
该CMakeLists.txt文件用于构建一个ROS节点，其中包括了构建依赖项的设置、构建目标的添加、安装目录的指定等。还使用了clang工具进行代码检查。
# 设置CMake最低版本为3.0.2
cmake_minimum_required(VERSION 3.0.2)
# 定义项目名为"ocs2_cartpole_ros"
project(ocs2_cartpole_ros)
# 生成compile_commands.json以支持Clang工具链的静态分析和格式检查
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
# 定义该项目依赖的catkin包列表
set(CATKIN_PACKAGE_DEPENDENCIES
ocs2_core
ocs2_ddp
ocs2_mpc
ocs2_ros_interfaces
ocs2_robotic_tools
ocs2_cartpole
ocs2_robotic_assets
)
# 寻找并引入ROS catkin库及其指定的依赖组件
find_package(catkin REQUIRED COMPONENTS
roslib
${CATKIN_PACKAGE_DEPENDENCIES}
)
# 寻找并引入Boost库，要求包含system和filesystem组件
find_package(Boost REQUIRED COMPONENTS
system
filesystem
)
# 寻找并引入Eigen3库，要求版本至少为3.3且不使用模块方式查找
find_package(Eigen3 3.3 REQUIRED NO_MODULE)
###################################
## catkin特定配置部分 ##
###################################
# 配置catkin包信息，包括头文件目录、catkin依赖项和非catkin依赖项（如Boost）
catkin_package(
INCLUDE_DIRS
include
${EIGEN3_INCLUDE_DIRS}
CATKIN_DEPENDS
${CATKIN_PACKAGE_DEPENDENCIES}
DEPENDS
Boost
)
###########
## 构建阶段 ##
###########
# 添加编译时的包含目录
include_directories(
include
${EIGEN3_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
# 添加MPC节点可执行目标，源文件为CartpoleMpcNode.cpp
add_executable(cartpole_mpc
src/CartpoleMpcNode.cpp
)
# 添加依赖，确保在构建cartpole_mpc前先构建所有已导出的catkin目标
add_dependencies(cartpole_mpc
${catkin_EXPORTED_TARGETS}
)
# 链接所需的库到cartpole_mpc目标
target_link_libraries(cartpole_mpc
${catkin_LIBRARIES}
)
# 添加Dummy测试节点可执行目标，源文件为DummyCartpoleNode.cpp和CartpoleDummyVisualization.cpp
add_executable(cartpole_dummy_test
src/DummyCartpoleNode.cpp
src/CartpoleDummyVisualization.cpp
)
# 添加依赖，同样确保在构建cartpole_dummy_test前先构建所有已导出的catkin目标
add_dependencies(cartpole_dummy_test
${catkin_EXPORTED_TARGETS}
)
# 链接所需的库到cartpole_dummy_test目标
target_link_libraries(cartpole_dummy_test
${catkin_LIBRARIES}
)
#####################
### Clang Tooling ###
#####################
# 查找并使用cmake_clang_tools，如果找到则进行静态代码检查
find_package(cmake_clang_tools QUIET)
if(cmake_clang_tools_FOUND)
message(STATUS "Running clang tooling.")
# 对cartpole_mpc和cartpole_dummy_test应用clang工具，并设置相关参数
add_clang_tooling(
TARGETS
cartpole_mpc
cartpole_dummy_test
SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include
CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
CT_HEADER_EXCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/generated
CF_WERROR # 指定错误级别，将警告视为错误处理
)
endif(cmake_clang_tools_FOUND)
##############
## 安装阶段 ##
##############
# 将项目中的include目录下本项目的头文件安装到catkin指定的头文件安装路径
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
# 将编译生成的cartpole_mpc和cartpole_dummy_test可执行文件安装到catkin指定的二进制文件安装路径
install(TARGETS cartpole_mpc cartpole_dummy_test
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# 将launch和rviz目录下的资源文件安装到catkin指定的share文件安装路径
install(DIRECTORY launch rviz
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)这个CMakeLists.txt文件是一个用于ROS项目的构建脚本，其核心功能包括：
-
cmake_minimum_required: 设置所需的CMake最小版本为3.0.2。
-
project: 定义项目名为"ocs2_cartpole_ros"。
-
CMAKE_EXPORT_COMPILE_COMMANDS: 开启生成compile_commands.json文件，以便于Clang工具链进行静态代码分析和格式检查。
-
CATKIN_PACKAGE_DEPENDENCIES: 定义了该项目依赖的一系列catkin包。
-
find_package(catkin): 寻找并添加ROS catkin库及其指定的依赖包。
-
find_package(Boost) 和 find_package(Eigen3): 寻找并添加Boost和Eigen3库作为项目依赖。
-
catkin_package: 定义catkin包的内容，包括头文件目录、catkin依赖项以及非catkin依赖项（如Boost）。
-
include_directories: 添加包含路径到编译器搜索路径中。
-
add_executable: 定义并添加两个可执行目标（cartpole_mpc和cartpole_dummy_test），它们分别对应源码中的节点实现。
-
target_link_libraries: 将catkin和其他依赖库链接至上述可执行目标。
-
CLANG TOOLING部分: 如果找到cmake_clang_tools，则运行clang工具并对特定目标进行静态代码分析，并排除一些目录。
-
install: 安装指令定义了将哪些文件和目录安装到系统相应的位置，包括头文件、可执行文件及launch和rviz等资源文件。
3.10 package.xml
这段代码是一个XML格式的ROS（机器人操作系统）包描述符，描述了一个名为“ocs2_cartpole_ros”的ROS包，包括其版本、描述、维护人员、许可证、构建工具和依赖关系等信息。
<?xml version="1.0"?>
<!-- XML文档声明，版本为1.0 -->
<package format="2">
<!-- 描述一个ROS软件包的根元素，format属性指明了manifest文件格式版本 -->
<name>ocs2_cartpole_ros</name>
<!-- 定义软件包名称：ocs2_cartpole_ros -->
<version>0.0.0</version>
<!-- 定义软件包版本号：0.0.0 -->
<description>The ocs2_cartpole_ros package</description>
<!-- 提供软件包的简要描述，这里是"ocs2_cartpole_ros"包 -->
<!-- 维护人员列表，每个maintainer元素包含邮箱联系方式 -->
<maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer>
<maintainer email="jcarius@ethz.ch">Jan Carius</maintainer>
<maintainer email="rgrandia@ethz.ch">Ruben Grandia</maintainer>
<!-- 软件包使用的许可证信息，这里未明确指定（待定） -->
<license>TODO</license>
<!-- 构建工具依赖项，此例中使用catkin作为构建工具 -->
<buildtool_depend>catkin</buildtool_depend>
<!-- 编译时依赖项，这些依赖在编译本软件包时需要先被构建 -->
<build_depend>roslib</build_depend>
<!-- roslib是ROS的基础库 -->
<build_depend>cmake_clang_tools</build_depend>
<!-- cmake_clang_tools提供对CMake和Clang工具链的支持 -->
<!-- 运行时依赖项，运行本软件包时必须存在的其他ROS软件包 -->
<depend>ocs2_core</depend>
<!-- ocs2框架的核心模块 -->
<depend>ocs2_ddp</depend>
<!-- ocs2框架下的动态规划(Dynamic Differential Programming)相关模块 -->
<depend>ocs2_mpc</depend>
<!-- ocs2框架下的模型预测控制(Model Predictive Control)相关模块 -->
<depend>ocs2_ros_interfaces</depend>
<!-- ocs2与ROS交互的接口模块 -->
<depend>ocs2_robotic_tools</depend>
<!-- ocs2提供的通用机器人工具集 -->
<depend>ocs2_cartpole</depend>
<!-- 专门针对cartpole问题的ocs2模块 -->
<depend>robot_state_publisher</depend>
<!-- ROS中的节点，用于发布机器人的状态信息 -->
<depend>ocs2_robotic_assets</depend>
<!-- 可能包含ocs2框架下特定于机器人的资源或模型数据 -->
</package>这段XML代码是用于描述一个ROS（Robot Operating System）软件包的manifest文件，具体来说，该软件包名为"ocs2_cartpole_ros"，其内容如下：
-
版本信息：此软件包的版本号为0.0.0。
-
描述：软件包的简要描述为"The ocs2_cartpole_ros package"，表明它与ocs2框架下解决cartpole问题有关。
-
维护人员：共有三位维护者，分别是Farbod Farshidian、Jan Carius和Ruben Grandia，并提供了他们的电子邮件联系方式以便于问题咨询和反馈。
-
许可证：软件包采用的许可证类型尚未明确，标注为"TODO"，表示需要进一步填写实际使用的开源许可证。
-
构建工具依赖：指定使用"catkin"作为构建工具。
-
编译时依赖项（build_depend）：
- "roslib" 是ROS的基础库；
- "cmake_clang_tools" 提供CMake和Clang工具链的支持。
-
运行时依赖项（depend）：
- "ocs2_core"、"ocs2_ddp"、"ocs2_mpc" 和 "ocs2_ros_interfaces" 等是一系列ocs2框架下的核心模块和接口；
- "ocs2_robotic_tools" 和 "ocs2_cartpole" 提供了机器人相关工具及针对cartpole模型的特定功能；
- "robot_state_publisher" 用于在ROS中发布机器人的状态信息；
- "ocs2_robotic_assets" 可能包含了一些与机器人相关的资源或模型数据。
整个XML文件主要用来指导ROS系统如何正确管理和构建这个软件包以及所需的依赖环境
2410
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
