NLP非线性优化第一步Webots与CasAdi

华北舵狗王

发布时间 2022.09.30阅读数 3094 评论数 0

非线性优化和轨迹优化是目前四足机器人和仿生机器人非常重要的技术部分，前者如腾讯Max机器人演示的后空翻和跳跃都需要用到相应的轨迹优化技术，而求解这样的轨迹优化控制问题就需要非线性优化工具，比较令人熟悉的是ipopt或MATLAB中自带的优化工具，CasADi则是一个在此基础上构建非线性优化问题的工具，其可以采用模板的方式来描述优化问题，从而使得非专业编程人员也能用其来求解优化问题，其支持Matlab和Python二者的安装和使用都非常方便，但是对于机器人控制来说最终还是需要将其采用C++进行部署：

1 Visual Studio 2017 和Windows下的安装

在如何评价腾讯发布首个全自研多模态四足机器人(Max)？中简单提到了四足机器人跳跃优化的问题，针对类似这样的运动开发需要首先在仿真环境中进行验证，在四足机器人跳跃轨迹优化_Na-Cl的博客-CSDN博客中给出了跳跃问题对于的优化形式，作者也贴出了相应的Matlab仿真，通过运行和调节参数我们可以非常方便的得到对应轨迹和控制输出，将其保存为data数据就可以采用样机的控制器对其跟踪从而实现跳跃过程。

但如果想将该离线优化改变成在线的方式则需要在机器人控制器中重新部署优化问题，首先需要先在C++环境中重新移植代码，对于没有机器人的情况我们优选还是采用仿真，在Ubuntu下安装会相对容易一些，但要在Webots和Windows环境中采用CasADi库就比较麻烦，一是CasADi Windows下的安装教程比较少，而是在Webots下的应用更是罕见，通过多方检索终于找到了一些安装方法，并且通过不断尝试成功在Webots下实现了NLP的求解，这样为后续仿真平台的扩展也提供了更广阔的空间，目前唯一就是Rbdl还没在Webots下部署成功，但是其可以在Ubuntu下的Webots环境中运行，参考下面的简单安装过程，记录CasADi的运行过程：

https://github-wiki-see.page/m/casadi/casadi/wiki/Howto%3A-Use-CasADi-in-Cpp-using-Visual-Studiogithub-wiki-see.page/m/casadi/casadi/wiki/Howto%3A-Use-CasADi-in-Cpp-using-Visual-Studio

（1）安装Ipopt和CasADi

参考教程首先在https://github.com/coin-or/Ipopt/releases/tag/releases%2F3.13.3中下载VS2019预编译的Ipopt-3.13.3-win64-msvs2019-md.zip，并解压到C盘根目录，这样我安装的VS2017但是目前测试并不影响编译。

之后下载https://github.com/casadi/casadi/archive/refs/tags/3.5.5.zip同样解压到C盘更目录，并在其下创建一个run.txt添加如下代码，并将其后缀改成.bat，Visual Studio 15 2017是你安装的VS版本具体名称需要查一下2019和2017不太一样：

@echo Must be run in a prompt context that defines vcvars.
@LIT Dynamics

cmake -G "Visual Studio 15 2017" -A x64 -B build -DWITH_IPOPT=ON -DIPOPT_LIBRARIES:FILEPATH="C:/Ipopt-3.13.3-win64-msvs2019-md/lib/ipopt.dll.lib" -DIPOPT_INCLUDE_DIRS:PATH="C:/Ipopt-3.13.3-win64-msvs2019-md/include/coin-or"  -DCMAKE_INSTALL_PREFIX:PATH="C:/casadi-3.5.5"
cmake --build build --config Release
cmake --install build

在安装VS后采用其x64 Native Tools Command Prompt软件进入模拟的Unix环境，cd到CasADi目录下运行刚才的脚本run.bat则会进行编译，如果没有错误则会在下面创建casadi目录里面有编译好的dll和lib：

这样CasADi就算安装好了，剩下的是在VS中配置对Demo进行测定。

（2）VS2017的配置

在安装好后需要进一步配置VS2017，这里首先测试的优化问题如下：

#include <casadi/casadi.hpp>
using namespace casadi;
int test_casadi() {
cout << "casadi_test" << endl;

  // This is another way to define a nonlinear solver. Opti is new
  /*
   *    min  x1^2 + x2^2 + x3^2
   *    s.t.    6*x1 + 3&x2 + 2*x3 - p0 = 0
   *              p2*x1 + x2 - x3 - 1 = 0
   *              x1, x2, x3 >= 0
   */

  // Optimization variables
  SX x = SX::sym("x", 3);
  std::cout << "x:" << x << std::endl;

  // Parameters
  SX p = SX::sym("p", 2);
  std::cout << "p:" << p << std::endl;

  // Objective
  SX f = x(0) * x(0) + x(1) * x(1) + x(2) * x(2);
  std::cout << "f:" << f << std::endl;

  // Constraints
  SX g = vertcat(6 * x(0) + 3 * x(1) + 2 * x(2) - p(0), p(1) * x(0) + x(1) - x(2) - 1);
  std::cout << "g:" << g << std::endl;

  // Initial guess and bounds for the optimization variables
  vector<double> x0 = { 0.15, 0.15, 0.00 };
  vector<double> lbx = { 0.00, 0.00, 0.00 };
  vector<double> ubx = { inf, inf, inf };

  // Nonlinear bounds
  vector<double> lbg = { 0.00, 0.00 };
  vector<double> ubg = { 0.00, 0.00 };

  // Original parameter values
  vector<double> p0 = { 5.00, 1.00 };

  // NLP
  SXDict nlp = { { "x", x }, { "p", p }, { "f", f }, { "g", g } };

  // Create NLP solver and buffers
  Function solver = nlpsol("solver", "ipopt", nlp);
  std::map<std::string, DM> arg, res;

  // Solve the NLP
  arg["lbx"] = lbx;
  arg["ubx"] = ubx;
  arg["lbg"] = lbg;
  arg["ubg"] = ubg;
  arg["x0"] = x0;
  arg["p"] = p0;
  res = solver(arg);

  // Print the solution
  cout << "--------------------------------" << endl;
  cout << "Optimal solution for p = " << arg.at("p") << ":" << endl;
  cout << setw(30) << "Objective: " << res.at("f") << endl;
  cout << setw(30) << "Primal solution: " << res.at("x") << endl;
  cout << setw(30) << "Dual solution (x): " << res.at("lam_x") << endl;
  cout << setw(30) << "Dual solution (g): " << res.at("lam_g") << endl;

  return 0;
}

首先是包含目录：

库目录：

附加包含目录：

附加依赖项：

上述部分我也不确定对不对或者有没有多余的，基本都给加上了，编译则理论上在VS中就不会报错了，但是如果在Webots中运行则会找不到.dll库。

（3）Webots下的运行配置

在VS中编译通过并不意味着Webots下可以运行，针对这一块网上没有太多的内容，首先需要解决的一个问题是Webots找不到dll问题，这里最简单的方法是将casadi目录下所有的dll和lib都拷贝到Webots控制器项目目录下与控制器exe文件同级：

这次运行后会出现如下错误：

WARNING(".../casadi/core/plugin_interface.hpp:322: Assertion "handle!=nullptr" failed:
PluginInterface::load_plugin: Cannot load shared library 'libcasadi_nlpsol_ipopt.so':
(
Searched directories: 1. casadipath from GlobalOptions
2. CASADIPATH env var
3. PATH env var (Windows)
4. LD_LIBRARY_PATH env var (Linux)
5. DYLD_LIBRARY_PATH env var (osx)
A library may be 'not found' even if the file exists:
* library is not compatible (different compiler/bitness)
* the dependencies are not found
)

通过多次测试可以定位到时ipopt优化器没法找到而单独运行casadi数据定义等代码是可以的，这里需要在Windows系统变量中增加ipopt预编译的lib库文件目录让casadi进行索引：

这样我们测试下非线性求解器 Casadi (c++使用例子)中的例子1，在运行之后，终于可以看到Webots里打印优化的结果：

可以看到总共花了0.1s优化结果一致但是时间长了10倍尚不知道是啥问题（与打印中Total CPU secs in IPOPT (w/o function evaluations) = 0.104 一致尚不知道如何加快，可以看到问题求解很快而ipopt这部分自己的时间很长），不过对于我们初步实现在Webots下的开发起码开了个好头：