CasADi库C++用法整理学习---以NMPC代码为例

参考几个使用方法博客
1
官方文档写的很清楚

对SM，DM，XM数据类型疑惑。什么时候使用什么样的类型，还是都可以？

x = MX.sym(“x”)
这将创建一个 1×1 矩阵，即一个包含名为 x 的符号基元的标量。这只是显示名称，而不是标识符。多个变量可以具有相同的名称，但仍然不同。identifier 是返回值。您还可以通过向 SX.sym 提供其他参数来创建向量值或矩阵值符号变量：

在这里插入图片描述

#include <casadi/casadi.hpp>
using namespace casadi;
int main() {SX x = SX::sym("x");SX y = SX::sym("y",5);SX Z = SX::sym("Z",4,2)SX f = pow(x,2) + 10;f = sqrt(f);std::cout << "f: " << f << std::endl;return 0;
}//sqrt((10+sq(x)))

DM 与 SX 非常相似，但不同之处在于非零元素是数值，而不是符号表达式。语法也是相同的，除了 SX.sym 等函数没有等效项。
DM 主要用于将矩阵存储在 CasADi 中，并用作函数的输入和输出。它不用于计算密集型计算。

MX 类型允许像 SX 一样构建由一系列基本操作组成的表达式。但与 SX 不同的是，这些初等运算并不局限于标量一元或二进制运算（R→R 或 R×R→R）。相反，用于形成 MX 表达式的基本运算可以是通用的多个稀疏矩阵值输入、多个稀疏矩阵值输出函数：Rn1×m1×…×RnN×mN→Rp1×q1×…×RpM×qM

SX和，MX两者对比

请注意，结果仅包含使用 MX 符号的两次运算（一次乘法和一次加法），而 SX 等效项则有八次运算（结果矩阵的每个元素两次）。因此，在处理具有许多元素的自然向量或矩阵值运算时，MX 可能更经济。
您不能将 SX 对象与 MX 对象相乘，也不能执行任何其他操作来在同一表达式图中混合两者。但是，您可以在 MX 图中包含对 SX 表达式定义的 Function 的调用。

MPC 代码
注意：opti.subject_to，opti.minimize，MX::mtimes，opti.variable，opti.parameter都是怎么用的
总的来说：

opti.variable: 定义变量。
opti.parameter: 定义参考值，已知的常量。
opti.set_value: 设置参数的具体值。
opti.minimize: 定义目标函数。
opti.subject_to: 添加约束条件。
MX::mtimes: 执行矩阵乘法。

#include "mpc_tracking/mpc.h"Mpc::Mpc() {N_ = 10;dt_ = 0.1;u_max_ = 1;w_max_ = 0.5;vector<double> weights = {1,1,1,2,2}; //Q,Ru_min_ = - u_max_;w_min_ = - w_max_;Q_ = DM::zeros(3,3); //索引之前初始化sizeR_ = DM::zeros(2,2);setWeights(weights);kinematic_equation_ = setKinematicEquation();
}
Mpc::~Mpc() {}void Mpc::setWeights(vector<double> weights) {//cout << "setweights" << endl;Q_(0, 0) = weights[0];Q_(1, 1) = weights[1];Q_(2, 2) = weights[2];R_(0, 0) = weights[3];R_(1, 1) = weights[4];//R_(2, 2) = weights[5];//cout << "set weight finish" << endl;}Function Mpc::setKinematicEquation() {//cout << "set kinematic" << endl;MX x = MX::sym("x");MX y = MX::sym("y");MX theta = MX::sym("theta");MX state_vars = MX::vertcat({x, y, theta});  //将状态变量组合成一个垂直向量（列向量）。MX v = MX::sym("v");MX w = MX::sym("w");MX control_vars = MX::vertcat({v, w});  //控制变量control_vars是一个包含两个元素的垂直向量，分别是v（线速度）和w（角速度）//rhs means right hand sideMX rhs = MX::vertcat({v * MX::cos(theta), v * MX::sin(theta), w}); // 动力学方程return Function("kinematic_equation", {state_vars, control_vars}, {rhs});// MX x = MX::sym("x");// MX y = MX::sym("y");// MX theta = MX::sym("theta");// MX state_vars = MX::vertcat({x, y, theta});// MX u = MX::sym("u");// MX v = MX::sym("v");// MX w = MX::sym("w");// MX control_vars = MX::vertcat({u, v, w});// MX rhs = u * MX::cos(theta) - v * MX::sin(theta); //这个方程描述了一个更复杂的非线性系统，例如四轮独立驱动的车辆或具有横向控制输入的系统// rhs = MX::vertcat({rhs, u * MX::sin(theta) + v * MX::cos(theta), w});// return Function("kinematic_equation", {state_vars, control_vars}, {rhs});
}bool Mpc::solve(Eigen::Vector3d current_states, Eigen::MatrixXd desired_states) {//cout << "jinqiujiele" << endl;Opti opti = Opti();Slice all;MX cost = 0;X = opti.variable(3, N_ + 1);// 定义一个3行N_+1列的矩阵变量X 注意：是变量。//第一个时间步（索引为0）表示当前时刻的状态 后面N_个时间步表示未来N_个时刻的状态和U对应U = opti.variable(2, N_);MX x = X(0, all);MX y = X(1, all);MX theta = X(2, all);MX v = U(0, all);MX w = U(1, all);//MX w = U(2, all);MX X_ref = opti.parameter(3, N_ + 1);   // 定义一个3行N_+1列的参数矩阵X_ref 注意：是已知的常量MX X_cur = opti.parameter(3);DM x_tmp1 = {current_states[0], current_states[1], current_states[2]};opti.set_value(X_cur, x_tmp1);  //set current state opti.set_value 用于设置参数的具体值cout << "set current state success" << endl;//按列索引vector<double> X_ref_v(desired_states.data(), desired_states.data() + desired_states.size());//auto tp1 = std::chrono::steady_clock::now();DM X_ref_d(X_ref_v);//X_ref_d.resize(3, N_ + 1);//cout << "desired_states:" << desired_states << endl;//cout << "X_ref_v:" << X_ref_v << endl;//cout << "X_ref_d:" << X_ref_d << endl;// DM x_tmp2(3, N_ + 1);// for (int i = 0; i < 3; ++i) {//     for (int j = 0; j <= N_; ++j) {//         x_tmp2(i, j) = desired_states(i, j);//     }// }X_ref = MX::reshape(X_ref_d, 3, N_ + 1);//opti.set_value(X_ref, X_ref_d);  //set reference traj// auto tp2 = std::chrono::steady_clock::now();// cout <<"set trajectory time:" // << chrono::duration_cast<chrono::microseconds>(tp2 - tp1).count() // << "microseconds" << endl;//cout << "set reference traj success" << endl;//cout << "x_ref:" << X_ref.size() << endl;//set costfunctionfor (int i = 0; i < N_; ++i) {MX X_err = X(all, i) - X_ref(all, i); MX U_0 = U(all, i);//cout << "U_0 size:" << U_0.size() << endl;//cout << "cost size:" << cost_.size() << endl;cost += MX::mtimes({X_err.T(), Q_, X_err}); //目标函数是状态误差和控制输入的成本之和//cout << "cost size:" << cost_.size() << endl; cost += MX::mtimes({U_0.T(), R_, U_0});//cout << "cost size:" << cost_.size() << endl;}//cout << "cost size:" << cost_.size() << endl;//MX::mtimes 用于执行矩阵乘法，用于计算两个矩阵的乘积cost += MX::mtimes({(X(all, N_) - X_ref(all, N_)).T(), Q_,X(all, N_) - X_ref(all, N_)});  //终端项//cout << "cost:" << cost << endl;opti.minimize(cost); //opti.minimize 用于定义优化问题的目标函数//cout << "set cost success" << endl;//kinematic constrains opti.subject_to 用于添加约束条件到优化问题中for (int i = 0; i < N_; ++i) {vector<MX> input(2);input[0] = X(all, i);input[1] = U(all, i);MX X_next = kinematic_equation_(input)[0] * dt_ + X(all, i); //这里体现了模型预测控制，模型。  下一步的状态=通过模型计算的速度* dt_ +当前状态opti.subject_to(X_next == X(all, i + 1));  // 动力学约束}//init value 初始化，初始状态赋值第一列所有行opti.subject_to(X(all, 0) == X_cur);//第一个时间步（索引为0）表示当前时刻的状态//speed angle_speed limit 控制输入的约束opti.subject_to(0 <= v <= u_max_);opti.subject_to(w_min_ <= w <= w_max_);//set solvercasadi::Dict solver_opts; // 设置求解器选项solver_opts["expand"] = true; //MX change to SX for speed upsolver_opts["ipopt.max_iter"] = 100;solver_opts["ipopt.print_level"] = 0;solver_opts["print_time"] = 0;solver_opts["ipopt.acceptable_tol"] = 1e-6;solver_opts["ipopt.acceptable_obj_change_tol"] = 1e-6;opti.solver("ipopt", solver_opts);//auto sol = opti.solve();solution_ = std::make_unique<casadi::OptiSol>(opti.solve());return true;
}
vector<double> Mpc::getFirstU() {vector<double> res;auto first_v =  solution_->value(U)(0, 0);auto first_w = solution_->value(U)(1, 0);//cout << "first_u" << first_u << " " << "first_v" << first_v << endl;res.push_back(static_cast<double>(first_v));res.push_back(static_cast<double>(first_w));return res;
}vector<double> Mpc::getPredictX() {vector<double> res;auto predict_x = solution_->value(X);cout << "nomal" << endl;//cout << "predict_x size :" << predict_x.size() << endl;for (int i = 0; i <= N_; ++i) {res.push_back(static_cast<double>(predict_x(0, i)));res.push_back(static_cast<double>(predict_x(1, i)));}return res;
}