[on #720] add new package to use osqp from euslisp (sqp with hotstart and sparce matrix are enabled)

[Naoki-Hiraoka]

OSQPをeuslispから使うpackageです．

#720
に加えて，
hotstartによる高速化
sparce matrixの性質を利用することによる高速化
を可能にしました．

euslispが疎行列の型をサポートしていないため，探索変数の数をn,制約の数をmとすると，少なくともo(n^2 + nm)の時間はかかります．

https://github.com/jsk-ros-pkg/jsk_control/blob/master/eus_qp/euslisp/test-contact-wrench-opt.l
https://github.com/jsk-ros-pkg/jsk_control/blob/master/eus_qp/euslisp/test-model-predictive-control.l
がOSQPで動くことを確認しテストを追加しました．

[k-okada]

something wrong with indigo https://github.com/jsk-ros-pkg/jsk_control/pull/743/checks?check_run_id=2555776920

 Errors << eus_osqp:make /github/home/ros/ws_jsk_control/logs/eus_osqp/build.make.000.log
  CMake Error at /github/home/ros/ws_jsk_control/build/eus_osqp/osqpproj-prefix/src/osqpproj-stamp/osqpproj-configure.cmake:16 (message):
    Command failed: 1
  
     '/usr/bin/cmake' '-DDFLOAT=OFF' '-GUnix Makefiles' '/github/home/ros/ws_jsk_control/build/eus_osqp/osqpproj-prefix/src/osqpproj'
  
    See also
  
      /github/home/ros/ws_jsk_control/build/eus_osqp/osqpproj-prefix/src/osqpproj-stamp/osqpproj-configure-*.log
  
  
  
  make[2]: *** [osqpproj-prefix/src/osqpproj-stamp/osqpproj-configure] Error 1
  make[1]: *** [CMakeFiles/osqpproj.dir/all] Error 2
  make: *** [all] Error 2

[k-okada]

@tongtybj ここでも osqp が使われています．
osqpだけをjsk_3rdpartyに移して，aeiral robot からもそれを使うのが良いと思います．

[Naoki-Hiraoka]

osqpを https://github.com/JSKAerialRobot/aerial_robot_3rdparty/tree/master/3rdparty/osqp からjsk_controlに移していただけるのは助かります。

以前 https://github.com/JSKAerialRobot/aerial_robot_3rdparty/tree/master/3rdparty/osqp を見て、便利だから使いたいなと思ったのですが、aerial_robot_3rdparty 内にあるosqpeigenがいらなかった(本家の最新版のosqpeigenはcatkinに対応しているので名前が干渉する)のと、 nloptがjsk_3rdpartyのものと名前が干渉しそうな気がしたのとで、aerial_robot_3rdpartyからosqpだけコピペして使わせていただいていました。 https://github.com/Naoki-Hiraoka/qp_solvers/tree/master/osqp

自分はosqpを、

• https://github.com/Naoki-Hiraoka/qp_solvers/tree/master/osqp-cpp
• https://github.com/Naoki-Hiraoka/prioritized_qp/tree/master/prioritized_qp_osqp
• https://github.com/Naoki-Hiraoka/ik_solvers/tree/master/prioritized_inverse_kinematics_solver
• https://github.com/Naoki-Hiraoka/aik_solvers/tree/master/prioritized_acc_inverse_kinematics_solver
• https://github.com/Naoki-Hiraoka/eus_osqp
• https://github.com/Naoki-Hiraoka/eus_prioritized_inverse_kinematics

で使っています.

[tongtybj]

I have created a PR to merge OSQP in jsk_3rdparty: jsk-ros-pkg/jsk_3rdparty#442

@Naoki-Hiraoka
BTW, do you have any article or equation to explain the implementation of https://github.com/Naoki-
Hiraoka/aik_solvers/tree/master/prioritized_acc_inverse_kinematics_solver. Just curious.

[Naoki-Hiraoka]

@Naoki-Hiraoka
BTW, do you have any article or equation to explain the implementation of https://github.com/Naoki-Hiraoka/aik_solvers/tree/master/prioritized_acc_inverse_kinematics_solver. Just curious.

適切なcitationかはわからないのですが、

• Murotani, Kazuya, et al. "Comparative study of force control methods for bipedal walking using a force-sensitive hydraulic humanoid." Advanced Robotics 34.21-22 (2020): 1455-1471.

の4.1節の計算を、

• O. Kanoun, F. Lamiraux, P. -B. Wieber, F. Kanehiro, E. Yoshida and J. -P. Laumond, "Prioritizing linear equality and inequality systems: Application to local motion planning for redundant robots," 2009 IEEE International Conference on Robotics and Automation, Kobe, Japan, 2009, pp. 2939-2944, doi: 10.1109/ROBOT.2009.5152293.

の方法で行っています。