Quad_sdk四足机器人代码解读系列1-代码部署及测试运行

      陆续运行了champ、legged_control、quad_sdk等四足机器人开源控制框架，quad_sdk在崎岖地形，路径规划、运动控制、状态估计、系统仿真等方面具有较好的控制优势和全栈的技术实现，为了实现自己的机器人梦，计划尝试建造自己的四足机器人，并将其作为自己四足机器人的代码控制框架，工作之余开始代码解读并记录之。

      工作第一阶段，记录部署与测试运行。

1、基础环境（已验证）

      操作系统：ubuntun20.04。

      ROS: noetic。

       显卡：NVIDIA 3070。

2、安装Ipopt

（1）安装依赖

   $sudo apt-get install gcc g++ gfortran git patch wget pkg-config liblapack-dev libmetis-dev libblas-dev

（2）创建一个存放所有跟Ipopt相关的文件夹，便于管理

    $mkdir ~/Ipopt_pkg

    $cd Ipopt_pkg

（3）安装ASL

     $git clone https://github.com/coin-or-tools/ThirdParty-ASL.git

    $cd ThirdParty-ASL

    $sudo ./get.ASL

    $sudo ./configure

    $sudo make

    $sudo make install

    $cd ..

（4）安装HSL

    $git clone https://github.com/coin-or-tools/ThirdParty-HSL.git

    $cd ThirdParty-HSL

    # 接下来需要下载coinhsl文件，并解压到ThirdParty-HSL目录下

   下载coinhsl文件，地址（https://github.com/CHH3213/testCPP/blob/master/coinhsl.zip）

   并解压后的coinhsl文件夹拷贝到ThirdParty-HSL目录下，在ThirdParty-HSL目录下，执行以下命令

   $sudo ./configure

   $sudo make

   $sudo make install

   $cd ..

（5）安装MUMPS

   $git clone https://github.com/coin-or-tools/ThirdParty-Mumps.git

   $cd ThirdParty-Mumps

   $sudo ./get.Mumps

   $sudo ./configure

  $sudo make

  $sudo make install

  $cd ..

（6）安装Ipopt

  $git clone https://github.com/coin-or/Ipopt.git

  $cd Ipopt

  $mkdir build

  $cd build

  $sudo ../configure

  $sudo make

  $sudo make test

  $sudo make install

（7）完善环境

  $cd /usr/local/include

  $sudo cp coin-or coin -r

  $sudo ln -s /usr/local/lib/libcoinmumps.so.3 /usr/lib/libcoinmumps.so.3

  $sudo ln -s /usr/local/lib/libcoinhsl.so.2 /usr/lib/libcoinhsl.so.2

  $sudo ln -s /usr/local/lib/libipopt.so.3 /usr/lib/libipopt.so.3

（8） Ipopt测试

  进入 IPOPT 源码文件夹如下位置，用官方例子测试

  $cd Ipopt/build/examples/Cpp_example

  $sudo make

  $./cpp_example

出现以下界面安装成功

***************************************************************************

This program contains Ipopt, a library for large-scale nonlinear optimization.

 Ipopt is released as open source code under the Eclipse Public License (EPL).

         For more information visit https://github.com/coin-or/Ipopt

***************************************************************************

This is Ipopt version 3.14.14, running with linear solver ma27.

Number of nonzeros in equality constraint Jacobian...:        2

Number of nonzeros in inequality constraint Jacobian.:        0

Number of nonzeros in Lagrangian Hessian.............:          2

Total number of variables............................:                 2

                     variables with only lower bounds:        0

                variables with lower and upper bounds:        1

                     variables with only upper bounds:        0

Total number of equality constraints.................:        1

Total number of inequality constraints...............:        0

        inequality constraints with only lower bounds:        0

   inequality constraints with lower and upper bounds:        0

        inequality constraints with only upper bounds:        0

iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls

   0 -2.5000000e-01 7.50e-01 5.00e-01  -1.0 0.00e+00    -  0.00e+00 0.00e+00   0

   1 -1.8981454e+00 6.12e-01 1.55e+00  -1.0 1.72e+00   0.0 6.27e-01 5.10e-01f  1

   2 -3.6781503e+00 1.32e-03 1.60e-01  -1.0 5.40e-01    -  1.00e+00 1.00e+00f  1

   3 -3.9900218e+00 1.67e-03 1.22e-02  -1.7 7.98e-02    -  1.00e+00 9.99e-01f  1

   4 -3.9998554e+00 1.56e-07 3.88e-06  -3.8 2.46e-03    -  1.00e+00 1.00e+00h  1

   5 -3.9999982e+00 3.16e-10 2.54e-09  -5.7 3.57e-05    -  1.00e+00 1.00e+00h  1

   6 -4.0000001e+00 5.28e-14 4.23e-13  -8.6 4.60e-07    -  1.00e+00 1.00e+00h  1

Number of Iterations....: 6

                                  (scaled)                 (unscaled)

Objective...............:  -4.0000000774945192e+00   -4.0000000774945192e+00

Dual infeasibility......:   4.2277292777725961e-13    4.2277292777725961e-13

Constraint violation....:   5.2846615972157451e-14    5.2846615972157451e-14

Variable bound violation:   9.6868413290707167e-09    9.6868413290707167e-09

Complementarity.........:   2.5056919964012917e-09    2.5056919964012917e-09

Overall NLP error.......:   2.5056919964012917e-09    2.5056919964012917e-09

Number of objective function evaluations             = 7

Number of objective gradient evaluations             = 7

Number of equality constraint evaluations            = 7

Number of inequality constraint evaluations          = 0

Number of equality constraint Jacobian evaluations   = 7

Number of inequality constraint Jacobian evaluations = 0

Number of Lagrangian Hessian evaluations             = 6

Total seconds in IPOPT                               = 0.001

EXIT: Optimal Solution Found.

*** The problem solved in 6 iterations!

*** The final value of the objective function is -4.

3、quad_sdk安装

  $mkdir  -p ~/Quad-SDK/src

  $cd  Quad-SDK/src

  $catkin_init_workspace   #创建工作空间

  $source devel/setup.bash    #添加启动文件

  $git clone https://github.com/robomechanics/quad-sdk.git #克隆源代码

  $cd quad-sdk

  $git checkout devel   # for melodic, checkout main, for noetic, checkout devel 检查环境

将Ipopt_pkg/ThirdParty-HSL/coinhsl文件夹拷贝到，Quad-sdk/src/quad-sdk/external/ipopt文件夹下。

$chmod +x setup.sh && ./setup.sh

$cd ~/Quad-SDK

$catkin build

4、系统运行

（1）启动模拟：

$roslaunch quad_utils quad_gazebo.launch

（2）机器人站立状态：

$rostopic pub /robot_1/control/mode std_msgs/UInt8 "data: 1"

（3）使用 twist input 运行控制：

$roslaunch quad_utils quad_plan.launch reference:=twist logging:=true

$rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/robot_1/cmd_vel

（4）使用全局规划器运行控制：

$roslaunch quad_utils quad_plan.launch reference:=gbpl logging:=true

相关知识背景。ROS