（9）NMPC非线性模型预测控制及机械臂ROS控制器实现

前言

本篇介绍Nonlinear Model Predictive Control，非线性模型预测控制，MPC是一种现代先进的控制方法，而NMPC特指对非线性模型的控制，其核心思想是在每个控制周期内利用系统的非线性模型及损失函数，预测未来一段时间内的系统行为，通过求解有限时域内的最优控制问题，实时生成最优控制输入。NMPC能够处理复杂约束和多变量系统，目前在无人机、自动驾驶领域广泛使用，随着目前硬件算力的提升，逐步地在机器人上也开始逐渐有所应用。

可以发现其强大之处不仅在于精准度，还有极快的响应速度，且多次测试后发现其不像先前介绍的几种控制器那样在极端状态下会失稳乱转，最多是在固定位置抖动。

1. NMPC介绍

1.1 最优控制

最优控制研究如下问题（离散形式）：

$\min_{\{u_k\}_{k=0}^{N-1},\{x_k\}_{k=0}^{N}} J = \sum_{k=0}^{N-1} L(x_k, u_k, k) + \Phi(x_N) \\ s.t. \quad \quad \quad \quad \quad x_{k+1} = f(x_k, u_k), \\ . \quad \quad \quad \quad \quad \quad \quad x_0 = x_{\text{init}} \\ . \quad \quad \quad \quad \quad \quad \quad x_{min}\leq x_k \leq x_{max} \\ . \quad \quad \quad \quad \quad \quad \quad u_{min}\leq u_k \leq u_{max}$

即找到最优控制策略，使得在满足约束：系统动态方程、初始条件、状态约束、控制约束及其他约束的情况下，找到最优的控制策略u及状态轨迹x（0->N），使得cost function J值最小。这与RL的思路师出同门，RL在于找到最大奖励，而最优控制在于找到最小损失。

对于上述问题的求解，有一系列的数值优化方法，尤其对于convex问题甚至存在解析解，这部分内容很多，希望详细了解可以参考课程【最优控制 16-745 2024】卡耐基梅隆—中英字幕_哔哩哔哩_bilibili

1.2 贝尔曼最优性理论

Bellman Principle指的是从当前状态出发的最优策略，其任何一个未来时刻的剩余策略也一定是从那一时刻出发的全局最优策略。

大意就是，如果我知道从A到E，走A->C->E->B->D最近，那么从C出发，也一定是走C->E->B->D最近，否则如果变成了C->B->E->D最近的话，那么A到E的路径就应该是A->C->B->E->D，与前提矛盾。

这意味着不论取全局最优轨迹其中的任意一段轨迹，这段轨迹也是局部最优的。

1.3 NMPC

对于1.1提到的最优控制问题，其存在两个问题：

1. 直接生成整条轨迹时，没有反馈机制（用Riccati递归除外），鲁棒性、稳定性差

2. 直接计算0->N，求解计算量大，无法用于实时控制

所以MPC是一种“退而求其次”的思想，想象整条轨迹N，直接求解最优问题得到的全局最优轨迹，而MPC的思想是每轮控制循环只取未来的N_norizon个轨迹点，一般是10-20个，求解如下问题：

$\min_{\{u_k\}_{k=0}^{N_{horizon}-1},\{x_k\}_{k=0}^{N_{horizon}}} J = \sum_{k=0}^{N-1} L(x_k, u_k, k) + \Phi(x_{N_{horizon}}) \\ s.t. \quad \quad \quad \quad \quad x_{k+1} = f(x_k, u_k), \\ . \quad \quad \quad \quad \quad \quad \quad x_0 = x_{\text{init}} \\ . \quad \quad \quad \quad \quad \quad \quad x_{min}\leq x_k \leq x_{max} \\ . \quad \quad \quad \quad \quad \quad \quad u_{min}\leq u_k \leq u_{max}$

之后每轮循环，只执行优化所得的第一个控制向量u0，根据贝尔曼最优性原理，u0一定是当前状态下的最优策略。而因为MPC是不断取局部N_norizon个轨迹点，其生成的最终控制策略是局部最优，而非全局最优。

而非线性MPC与线性MPC的最大区别在于，系统的动态方程 $x_{k+1} = f(x_k, u_k)$ 无法线性化表达为 $x_{k+1} =Ax_k+Bu_k$ （如果可以的话，由于cost function J一般取为二次型，其最优问题求解问题变为convex QP，存在解析解，求解过程极简）（虽然也可以通过局部一阶泰勒展开，但是对于强非线性系统，精准度很差），求解难度更高。

1.4 机械臂的NMPC形式

取 $x = \begin{Bmatrix} q\\ \dot{q} \end{Bmatrix}$ ，之后取

$\min_{\{u_k\}_{k=0}^{N_{horizon}-1},\{x_k\}_{k=0}^{N_{horizon}}} J = \sum_{k=0}^{N-1} x_k^TQx_k + u_k^TRu_k +x_{N_{horizon}}^TQ_fx_{N_{horizon}} \\\\ s.t. \quad \quad \quad \quad \quad x_{k+1} = f(x_k, u_k), \\ . \quad \quad \quad \quad \quad \quad \quad x_0 = x_{\text{init}} \\ . \quad \quad \quad \quad \quad \quad \quad x_{min}\leq x_k \leq x_{max} \\ . \quad \quad \quad \quad \quad \quad \quad u_{min}\leq u_k \leq u_{max}$

$x_{k+1} = f(x_k, u_k)$ 可以用欧拉法或RK4对机械臂的状态空间方程离散得到，后文ROS控制器实现时介绍。

2. ROS控制器实现

相对于前几篇介绍过的控制方法，在ROS控制器中实现NMPC过程非常复杂，实际上跟ros controller关系都不大了，基本上全程在调用外部函数，下面尽可能详细介绍。

2.1 使用pinocchio&casadi导出机械臂动态方程

在C++中用pinocchio读取机械臂urdf，用pinocchio::casadi::sym定义q、q_dot、tau为变量，调用pinocchio::crba和pinocchio::nonLinearEffects拼出标准动态方程，之后用casadi导出动态方程（连续形式），详见：

#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/autodiff/casadi.hpp>
#include <casadi/casadi.hpp>
#include <Eigen/Dense>
#include <iostream>// 工具函数：Eigen::Matrix<SX, -1, 1> 转 std::vector<SX>
template <typename Derived>
std::vector<casadi::SX> eigen_to_stdvector(const Eigen::MatrixBase<Derived>& m) {std::vector<casadi::SX> v(m.size());for (int i = 0; i < m.size(); ++i)v[i] = m(i);return v;
}template <typename Derived>
std::vector<std::vector<casadi::SX>> eigenmat_to_stdvectorvec(const Eigen::MatrixBase<Derived>& m) {std::vector<std::vector<casadi::SX>> v(m.rows());for (int i = 0; i < m.rows(); ++i) {v[i].resize(m.cols());for (int j = 0; j < m.cols(); ++j)v[i][j] = m(i, j);}return v;
}int main() {std::string urdf_path = "/home/mr/myproject/src/my_mujoco_test/urdf/my_mujoco_arm_adaptive.urdf";pinocchio::Model model;pinocchio::urdf::buildModel(urdf_path, model);using SX = casadi::SX;using VectorXsx = Eigen::Matrix<SX, Eigen::Dynamic, 1>;using MatrixXsx = Eigen::Matrix<SX, Eigen::Dynamic, Eigen::Dynamic>;VectorXsx q(model.nq), v(model.nv), tau(model.nv);pinocchio::casadi::sym(q, "q");pinocchio::casadi::sym(v, "v");pinocchio::casadi::sym(tau, "tau");auto model_sx = model.cast<SX>();pinocchio::DataTpl<SX> data_sx(model_sx);MatrixXsx M = pinocchio::crba(model_sx, data_sx, q);VectorXsx nle = pinocchio::nonLinearEffects(model_sx, data_sx, q, v);// 用CasADi的solvecasadi::SX M_casadi = casadi::SX::zeros(M.rows(), M.cols());for (int i = 0; i < M.rows(); ++i)for (int j = 0; j < M.cols(); ++j)M_casadi(i, j) = M(i, j);casadi::SX tau_minus_nle = casadi::SX::vertcat(eigen_to_stdvector(tau - nle));casadi::SX vdot_casadi = casadi::SX::solve(M_casadi, tau_minus_nle);// 拼接x, xdotcasadi::SX x_casadi = casadi::SX::vertcat(eigen_to_stdvector(q));x_casadi = casadi::SX::vertcat({x_casadi, casadi::SX::vertcat(eigen_to_stdvector(v))});casadi::SX xdot_casadi = casadi::SX::vertcat(eigen_to_stdvector(v));xdot_casadi = casadi::SX::vertcat({xdot_casadi, vdot_casadi});casadi::SX u_casadi = casadi::SX::vertcat(eigen_to_stdvector(tau));casadi::Function f_dyn("f_dyn", {x_casadi, u_casadi}, {xdot_casadi}, {"x", "u"}, {"xdot"});f_dyn.save("robot_dynamics.casadi");std::cout << "Saved CasADi function robot_dynamics.casadi" << std::endl;return 0;
}

运行后会生成一个插件，根据命名比如是 my_robot_dynamics，在终端运行这个插件（./my_robot_dynamics）会在运行目录生成robot_dynamics.casadi。

2.2 使用acados定制NLP求解器

如下，用python使用acados生成NLP求解器，包装为C++程序供ROS控制器使用。

读取刚刚生成的robot_dynamics.casadi，定义N_horizon、Tf、状态权重Q、输入权重R、终端状态权重Q_f，注意以下几点：

1. N_horizon一般取10-20，根据计算性能取，数字越大预测全局性越好，但求解耗时越长

2. Tf为预测长度的总时长，比如我取N_horizon=10，Tf=0.1，相当于每轮求解预测未来0.1s，采样频率100Hz，这个频率要小于等于ros控制器的循环频率，否则会出现“跳点”导致剧烈抖动

3. 一般取R比Q小一些，Q/R的比值越大响应速度越快，但过大可能出现抖动，反之则越柔顺，但响应速度会受到影响

4. 一般取Qf为Q的5-10倍

5. acados定义求解器时，N_horizon、Tf作为结构参数是不支持对外暴露接口的，即不能在调用函数时通过外部传参更改，一旦生成就定死了。所以即使Q、R、Qf、状态/输入约束可以，我也干脆都写死了，每次调试重新运行程序生成C++程序就好了。

6. 离散方法选择“ERK”，RK4方法，离散精度较高

import casadi as ca
import numpy as np
from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModelf_dyn = ca.Function.load("robot_dynamics.casadi")
nq, nv = 6, 6model = AcadosModel()
model.x = ca.MX.sym('x', nq + nv)
model.u = ca.MX.sym('u', nv)
model.f_expl_expr = f_dyn(model.x, model.u)
model.name = "robot_nmpc"ocp = AcadosOcp()
ocp.model = model
N_horizon = 10
Tf = 0.1
ocp.dims.N = N_horizon
ocp.solver_options.tf = Tf
# 输入约束
u_min = -100 * np.ones(nv)
u_max =  100 * np.ones(nv)
u_min[3] = -40
u_min[4] = -40
u_min[5] = -40
u_max[3] = 40
u_max[4] = 40
u_max[5] = 40
ocp.constraints.lbu = u_min
ocp.constraints.ubu = u_max
ocp.constraints.idxbu = np.arange(nv)
# 状态约束
idxbx = np.array([6,7,8])              # 约束前6个分量（关节位置）
lbx = np.array([-3.0, -3.0, -3.0])  # 下界
ubx = np.array([ 3.0,  3.0,  3.0])  # 上界
ocp.constraints.idxbx = idxbx
ocp.constraints.lbx = lbx
ocp.constraints.ubx = ubxQ = 200 * np.eye(nq + nv)
for i in range(nv):Q[i + nq, i + nq] = 10  # 只对速度项施加较小的权重
Q[3,3] = 100
Q[4,4] = 100
Q[5,5] = 100
Q[9,9] = 5
Q[10,10] = 5
Q[11,11] = 5
R = 250 * np.eye(nv)
R[3,3] = 50
R[4,4] = 50
R[5,5] = 50
Q_terminal = Q * 10  
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.W = Q
ocp.cost.W_e = Q_terminal
ocp.cost.Vx = np.eye(nq + nv)
ocp.cost.Vu = np.zeros((nq + nv, nv))
ocp.cost.Vx_e = np.eye(nq + nv)
ocp.cost.yref = np.zeros(nq + nv)
ocp.cost.yref_e = np.zeros(nq + nv)
ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES'
ocp.solver_options.integrator_type = 'ERK'  # 使用RK4方法离散动态方程
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
# ocp.solver_options.nlp_tol = 1e-6         # NLP全局容差
# ocp.solver_options.nlp_tol_eq = 1e-6      # 等式约束容差
# ocp.solver_options.nlp_tol_ineq = 1e-6    # 不等式约束容差
# ocp.solver_options.qp_tol = 1e-8          # QP子问题容差
# 先占位（必须有，哪怕初值不重要）
ocp.constraints.x0 = np.zeros(nq + nv)solver = AcadosOcpSolver(ocp, json_file="acados_ocp.json")def nlmpc_acados_step(x0, x_ref_traj):solver.reset()solver.set(0, "x", x0)for i in range(N_horizon):solver.set(i, "yref", x_ref_traj[i])solver.set(N_horizon, "yref", x_ref_traj[-1])   status = solver.solve()if status != 0:print(f"acados solve failed: {status}")u0 = solver.get(0, "u")return u0if __name__ == "__main__":x0 = np.zeros(nq + nv)x_ref_traj = np.tile(np.concatenate([np.ones(nq), np.zeros(nv)]), (N_horizon+1, 1))u0 = nlmpc_acados_step(x0, x_ref_traj)print("MPC optimal u0:", u0)

运行以上程序，将会生成如下一系列文件

之后才cmakelist.txt中将生成的程序都导入到环境中

add_library(${PROJECT_NAME}SHAREDcontroller/my_mpc_controller.cppcontroller/c_generated_code/acados_solver_robot_nmpc.ccontroller/c_generated_code/acados_sim_solver_robot_nmpc.ccontroller/c_generated_code/robot_nmpc_model/robot_nmpc_expl_ode_fun.ccontroller/c_generated_code/robot_nmpc_model/robot_nmpc_expl_vde_forw.ccontroller/c_generated_code/robot_nmpc_model/robot_nmpc_expl_vde_adj.c
)

2.3 ROS2控制器调用NLP求解器

在头文件中增加定义：

int N_mpc_;
double Tf_;
std::vector<Eigen::VectorXd> X_ref_window_;
robot_nmpc_solver_capsule *acados_ocp_capsule_;
trajectory_msgs::msg::JointTrajectoryPoint interp_pt_;
bool dummy_reached_;
double t_query_sec_;
rclcpp::Duration t_query_{rclcpp::Duration::from_seconds(0.0)};

在控制器onit部分对上述变量进行初始化，其中robot_nmpc_solver_capsule *acados_ocp_capsule_;就是我们刚刚生成的求解器对象的指针。

虽然在这里定义的N_mpc和Tf不能改变NLP求解器中的值，但是后续要供其他变量使用，所以提前定义好。

在on_active处创建求解器

acados_ocp_capsule_ = robot_nmpc_acados_create_capsule();
if (robot_nmpc_acados_create(acados_ocp_capsule_) != 0) {RCLCPP_ERROR(get_node()->get_logger(), "Failed to create acados solver");return controller_interface::CallbackReturn::ERROR;
}

在update中调用，关键在于如下这些NLP求解器的初始化过程，其参数命名等都是固定的，只需要把X_window_ref_（每轮mpc计算的参考轨迹段）和xic_（每轮mpc求解的初始位置，即当前位置）填入即可。

        ocp_nlp_in *nlp_in = robot_nmpc_acados_get_nlp_in(acados_ocp_capsule_);ocp_nlp_out *nlp_out = robot_nmpc_acados_get_nlp_out(acados_ocp_capsule_);ocp_nlp_config *nlp_config = robot_nmpc_acados_get_nlp_config(acados_ocp_capsule_);ocp_nlp_dims *nlp_dims = robot_nmpc_acados_get_nlp_dims(acados_ocp_capsule_);// 初始约束ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", xic.data());ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", xic.data());// 设置 yref (轨迹参考)for (int i = 0; i < X_ref_window_.size()-1; ++i) {ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", X_ref_window_[i].data());}// 终端参考ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, X_ref_window_.size(), "yref", X_ref_window_.back().data());int status = robot_nmpc_acados_solve(acados_ocp_capsule_);if (status != 0) {RCLCPP_WARN(get_node()->get_logger(), "acados solve failed: %d", status);}std::vector<double> u0(joint_names_.size());ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", u0.data());

详见具体程序：

    //update函数在控制器被激活后每次更新时调用，主要用于处理实时数据和执行控制逻辑，属于主函数controller_interface::return_type MyMpcController::update(const rclcpp::Time & time, const rclcpp::Duration & period){   dt_ = period.seconds() + (period.nanoseconds() * 1e-9); // 计算时间间隔，单位为秒double effort_command = 0.0; // 初始化关节力矩命令Eigen::VectorXd zeros = Eigen::VectorXd::Zero(joint_names_.size());Eigen::VectorXd double_zeros = Eigen::VectorXd::Zero(joint_names_.size() * 2);bool reached_end = false;// 获取 acados 内部结构体ocp_nlp_in *nlp_in = robot_nmpc_acados_get_nlp_in(acados_ocp_capsule_);ocp_nlp_out *nlp_out = robot_nmpc_acados_get_nlp_out(acados_ocp_capsule_);ocp_nlp_config *nlp_config = robot_nmpc_acados_get_nlp_config(acados_ocp_capsule_);ocp_nlp_dims *nlp_dims = robot_nmpc_acados_get_nlp_dims(acados_ocp_capsule_);if (new_msg_){trajectory_msg_ = *traj_msg_external_point_ptr_.readFromRT(); // 从实时缓冲区读取完整的轨迹信息start_time_ = time; // 记录控制器开始时间new_msg_ = false; // 重置新消息标志位}for (size_t i = 0; i < joint_effort_command_interfaces_.size(); ++i) {last_torques_[i] = joint_effort_command_interfaces_[i].get().get_value(); // 保存上一次的关节力矩}if (trajectory_msg_ != nullptr) // 如果轨迹不为空{   // 到位检查double cur_time_sec = (time - start_time_).seconds();double total_time = trajectory_msg_->points.back().time_from_start.sec +trajectory_msg_->points.back().time_from_start.nanosec * 1E-9;if (cur_time_sec >= total_time){reached_end = true; // 如果当前插值点位置等于轨迹最后一个点的位置，则认为到达末尾}if (!reached_end) {double dt_mpc = Tf_ / double(N_mpc_);X_ref_window_.clear();for (size_t k = 0; k < N_mpc_; ++k) {t_query_sec_ = (time - start_time_).seconds() + k * dt_mpc;t_query_ = rclcpp::Duration::from_seconds(t_query_sec_);interpolate_trajectory_point(*trajectory_msg_, t_query_, interp_pt_, dummy_reached_);Eigen::VectorXd x_ref(joint_names_.size() * 2);for (size_t j = 0; j < joint_names_.size(); ++j) {x_ref(j) = interp_pt_.positions[j];x_ref(j + joint_names_.size()) = (interp_pt_.velocities.size() > j) ? interp_pt_.velocities[j] : 0.0;}X_ref_window_.push_back(x_ref);if (k == 0) {point_interp_ = interp_pt_; // 更新当前插值点}}} else {// 如果轨迹执行到达末尾却没有到达目标位置，使用最后一个点的值填充参考轨迹point_interp_ = trajectory_msg_->points.back();X_ref_window_.clear();// 没有新目标，保持当前位置for (size_t i = 0; i < joint_names_.size(); ++i) {X_ref_[i] = point_interp_.positions[i]; // 获取期望关节位置X_ref_[i + joint_names_.size()] = 0.0; // 获取期望关节速度}X_ref_window_.clear();for (size_t k = 0; k < N_mpc_; ++k) {X_ref_window_.push_back(X_ref_);}}// 调用MPC控制算法，计算关节力矩命令Eigen::VectorXd xic = Eigen::VectorXd::Zero(joint_names_.size() * 2);for (size_t i = 0; i < joint_names_.size(); ++i) {xic[i] = joint_position_state_interfaces_[i].get().get_value(); // 获取当前关节位置xic[i + joint_names_.size()] = joint_velocity_state_interfaces_[i].get().get_value(); // 获取当前关节速度}// 初始约束ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", xic.data());ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", xic.data());// 设置 yref (轨迹参考)for (int i = 0; i < X_ref_window_.size()-1; ++i) {ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", X_ref_window_[i].data());}// 终端参考ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, X_ref_window_.size(), "yref", X_ref_window_.back().data());int status = robot_nmpc_acados_solve(acados_ocp_capsule_);if (status != 0) {RCLCPP_WARN(get_node()->get_logger(), "acados solve failed: %d", status);}std::vector<double> u0(joint_names_.size());ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", u0.data());for (size_t i = 0; i < joint_names_.size(); ++i) {effort_command =  u0[i]; // 计算关节力矩命令if (effort_command - last_torques_[i] > max_delta_[i]) effort_command = last_torques_[i] + max_delta_[i]; // 限制关节力命令的变化量else if (effort_command - last_torques_[i] < -max_delta_[i]) effort_command = last_torques_[i] - max_delta_[i]; // 限制关节力命令的变化量joint_effort_command_interfaces_[i].get().set_value(effort_command); // 设置关节力命令 }if (active_goal_ && reached_end){bool all_joints_reached = true;for (size_t i = 0; i < joint_names_.size(); ++i) {double pos_actual = joint_position_state_interfaces_[i].get().get_value();double pos_desired = point_interp_.positions[i];double error = std::abs(pos_actual - pos_desired);if (error > goal_tolerance_[i]) {all_joints_reached = false;break;}}if (all_joints_reached) {RCLCPP_INFO(get_node()->get_logger(), "All joints reached goal, sending SUCCEED to action server.");auto feedback = std::make_shared<control_msgs::action::FollowJointTrajectory::Feedback>();feedback->joint_names = joint_names_;feedback->desired = point_interp_;active_goal_->publish_feedback(feedback);auto result = std::make_shared<control_msgs::action::FollowJointTrajectory::Result>();result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL;active_goal_->succeed(result);trajectory_msg_.reset();active_goal_.reset();}}}else{// 没有新目标，保持当前位置for (size_t i = 0; i < joint_names_.size(); ++i) {X_ref_[i] = point_interp_.positions[i]; // 获取期望关节位置X_ref_[i + joint_names_.size()] = 0.0; // 获取期望关节速度}X_ref_window_.clear();for (size_t k = 0; k < N_mpc_; ++k) {X_ref_window_.push_back(X_ref_);}Eigen::VectorXd xic = Eigen::VectorXd::Zero(joint_names_.size() * 2);for (size_t i = 0; i < joint_names_.size(); ++i) {xic[i] = joint_position_state_interfaces_[i].get().get_value(); // 获取当前关节位置xic[i + joint_names_.size()] = joint_velocity_state_interfaces_[i].get().get_value(); // 获取当前关节速度}// 调用MPC控制算法，计算关节力矩命令// 设置 x0// 只需要设置一次初始状态约束（"lbx"/"ubx"）如果你的模型有 x0 约束ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", xic.data());ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", xic.data());for (int i = 0; i < X_ref_window_.size(); ++i) {ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", X_ref_window_[i].data());}// 终端参考ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, X_ref_window_.size(), "yref", X_ref_window_.back().data());int status = robot_nmpc_acados_solve(acados_ocp_capsule_);if (status != 0) {RCLCPP_WARN(get_node()->get_logger(), "acados solve failed: %d", status);}            // 求解std::vector<double> u0(joint_names_.size());ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", u0.data());RCLCPP_INFO_STREAM(get_node()->get_logger(), "u0: " << Eigen::Map<Eigen::VectorXd>(u0.data(), u0.size()).transpose());for (size_t i = 0; i < joint_names_.size(); ++i) {effort_command =  u0[i] ; // 计算关节力矩命令if (effort_command - last_torques_[i] > max_delta_[i]) effort_command = last_torques_[i] + max_delta_[i]; // 限制关节力命令的变化量else if (effort_command - last_torques_[i] < -max_delta_[i]) effort_command = last_torques_[i] - max_delta_[i]; // 限制关节力命令的变化量joint_effort_command_interfaces_[i].get().set_value(effort_command); // 设置关节力命令 }}// 发布当前力距命令effort_msg_pub_.header.stamp = time;effort_msg_pub_.name = joint_names_;effort_msg_pub_.effort.clear();for (const auto & interface : joint_effort_command_interfaces_) {effort_msg_pub_.effort.push_back(interface.get().get_value());}effort_command_publisher_->publish(effort_msg_pub_);// 发布当前轨迹指令trajectory_msg_pub_.header.stamp = time; // 正确：设置时间戳trajectory_msg_pub_.joint_names = joint_names_;trajectory_msg_pub_.points.clear(); // 清空旧点，避免累加trajectory_msg_pub_.points.push_back(point_interp_); // 正确：添加插值点joint_command_publisher_->publish(trajectory_msg_pub_); // 发布当前轨迹指令return controller_interface::return_type::OK; // 返回更新成功}