Add ik check

[Kanazawanaoaki]

IKが難しそうなおたまを棚に出し入れする(move-rarm-to-ladle-at-shelf)でIKチェックシステムの例を作ってみました．

$ roscd jsk_2021_10_soup_from_boil/euslisp/check/
$ roseus put-and-take-ladle-in-shelf-ik-check.l 
(check)

とすると，

---- IK check result ----- 
0 ik fail at #<coordinates #X55ca829c3ef8  1044.0 8528.0 30000.0 / 3.037 0.0 0.0> 
2 ik fail at #<coordinates #X55ca829c3fb8  1019.0 8546.0 30000.0 / 3.142 0.0 0.0> 
0 ik fail at #<coordinates #X55ca829c4078  1087.0 8510.0 30000.0 / -3.019 0.0 0.0> 
0 ik fail at #<coordinates #X55ca829c4138  1096.0 8535.0 30000.0 / -3.054 0.0 0.0> 
0 ik fail at #<coordinates #X55ca829c3988  1071.0 8471.0 30000.0 / -3.072 0.0 0.0> 
1 ik fail at #<coordinates #X55ca829c2c08  1034.0 8500.0 30000.0 / -3.089 0.0 0.0> 
2 ik fail at #<coordinates #X55ca829c1e88  1030.0 8544.0 30000.0 / -3.037 0.0 0.0> 
0 ik fail at #<coordinates #X55ca829c1108  1085.0 8460.0 30000.0 / 3.089 0.0 0.0> 
2 ik fail at #<coordinates #X55ca829c4168  1008.0 8495.0 30000.0 / 3.107 0.0 0.0> 
2 ik fail at #<coordinates #X55ca829c4228  1016.0 8532.0 30000.0 / -3.054 0.0 0.0> 
2 ik fail at #<coordinates #X55ca829c42e8  1007.0 8522.0 30000.0 / -3.002 0.0 0.0> 
0 ik fail at #<coordinates #X55ca82aa69b0  1027.0 8452.0 30000.0 / -3.124 0.0 0.0> 
0 ik fail at #<coordinates #X55ca82aa6a70  1045.0 8522.0 30000.0 / 3.072 0.0 0.0> 
1 ik fail at #<coordinates #X55ca82aa6b30  1068.0 8481.0 30000.0 / -3.072 0.0 0.0> 
0 ik fail at #<coordinates #X55ca82aa6bf0  1091.0 8480.0 30000.0 / -3.107 0.0 0.0> 
0 ik fail at #<coordinates #X55ca82aa6cb0  1059.0 8473.0 30000.0 / -3.037 0.0 0.0> 
0 ik fail at #<coordinates #X55ca82aa6d70  1085.0 8482.0 30000.0 / -2.985 0.0 0.0> 
0 ik fail at #<coordinates #X55ca82aa6e30  1079.0 8506.0 30000.0 / 3.124 0.0 0.0> 
2 ik fail at #<coordinates #X55ca829c43a8  1017.0 8499.0 30000.0 / -3.002 0.0 0.0> 
2 ik fail at #<coordinates #X55ca82aa51b0  1012.0 8463.0 30000.0 / -3.089 0.0 0.0> 
2 ik fail at #<coordinates #X55ca82aa5270  1007.0 8527.0 30000.0 / 3.124 0.0 0.0> 
2 ik fail at #<coordinates #X55ca82aa5330  1013.0 8473.0 30000.0 / -3.037 0.0 0.0> 
1 ik fail at #<coordinates #X55ca82aa53d8  1066.0 8505.0 30000.0 / -3.002 0.0 0.0> 
2 ik fail at #<coordinates #X55ca82aa5498  1010.0 8451.0 30000.0 / -3.089 0.0 0.0> 
2 ik fail at #<coordinates #X55ca82aa5558  1003.0 8480.0 30000.0 / -3.054 0.0 0.0> 
2 ik fail at #<coordinates #X55ca82aa5618  1022.0 8507.0 30000.0 / -3.107 0.0 0.0> 
0 ik fail at #<coordinates #X55ca82aa3728  1093.0 8474.0 30000.0 / -3.019 0.0 0.0> 
2 ik fail at #<coordinates #X55ca82aa37e8  1005.0 8476.0 30000.0 / -3.089 0.0 0.0> 
0 ik fail at #<coordinates #X55ca82aa38a8  1052.0 8523.0 30000.0 / 3.054 0.0 0.0> 
2 ik fail at #<coordinates #X55ca82aa3968  1028.0 8520.0 30000.0 / -3.089 0.0 0.0> 
(total) 31 ik fail at 17/30 spots 

のようにprintされるようになっています．
こんな感じでしょうか？問題点としては，現状だとプログラムのどの部分のIKが失敗しているかとかはわかりにくくなってしまっているのと，motionの方のプログラムを少し変えないと行けないので面倒です．

[708yamaguchi]

roseusのテストは、以下のようにdeftest、assertすると良いですね
CMakeLists.txtからテストを呼びやすくなります
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/test/default-ri-test.l
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/27066da325b7224bb35e3c4ded05b751c96fd907/pr2eus/CMakeLists.txt#L45-L64

実装に関してですが、自分の理想はdemo/soup-from-boil.lなどの中身を全く変えずに、それをwrapするテスト関数を書くだけでテストが出来ることかなと思います。
して、

例えば、
*pr2*の:inverse-kinematics関数を上書きして、上記のassert関数を入れるのとかどうでしょうか。
:inverse-kinematicsにassertを入れる例：

(load "package://pr2eus/pr2.l")

(defclass pr2-test-robot
  :super pr2-sensor-robot
  :slots ()
  )
(defmethod pr2-test-robot
  (:inverse-kinematics (&rest args)
    (assert (send-super* :inverse-kinematics args))))

(setq *pr2* (instance pr2-test-robot :init))

;; IK succeed
(send *pr2* :rarm :inverse-kinematics
      (make-coords :pos #f(700 -500 800)))

;; IK fail (assertion)
(send *pr2* :rarm :inverse-kinematics
      (make-coords :pos #f(700 -500 8000)))

テスト用roseusファイルの実装例

(require :unittest "lib/llib/unittest.l")
(load "package://jsk_2021_soup_from_boil/euslisp/demo/soup-from-boil.l")

(init-unit-test)

(deftest test-soup-from-boil-setup
  (setup)
)

(deftest test-soup-from-boil-demo
  (demo)
)

(run-all-tests)
(exit)

これを上記のリンクのようにCMakeLists.txtから呼べるようにすれば良いのかな、という予想です。
（より良い実装方法はギガや平岡くんに聞いたほうが良さそうですが。。。）