Add ik check

jsk_2021_10_soup_from_boil/euslisp/check/put-and-take-ladle-in-shelf-ik-check.l

@@ -0,0 +1,61 @@
+(load "package://jsk_2021_10_soup_from_boil/euslisp/motion/tool-at-arrange.l")
+(load "package://jsk_2021_10_soup_from_boil/euslisp/util/move-to-kitchen-with-map.l")
+
+;; IRT上でIKのチェックをする
+(ros::roseus "shelf_ik_check" :anonymous t)
+(move-on-irt :kitchen)
+
+(warn "please run (check) ~%")
+
+;; TODO random-coordsやmake-random-cds-list，checkも引数に関数（とpr2-spot-listも？）をとるようにしてチェックするようにして/util/ik-check.lとかに入れる？
+;; 他のIKが大事な部分もこれで出来るようにする． 今回はmove-rarm-to-ladle-at-shelfを少し変更して，IK失敗をカウントして返り値として返すようにして，:send-riの引数で*ri*に送るかどうかを指定できるようにして時短に．
+
+;; メモ IKでも場所によって重要度とかがある？
+
+(defun random-coords (initial-cds &key (x 100) (y 50) (theta 30))
+  (let (ans-cds x-num y-num theta-num)
+    (setq ans-cds (send initial-cds :copy-worldcoords))
+    (setq x-num (random x))
+    (if (= (random 2) 1)
+        (setq x-num (* x-num -1)))
+    (setq y-num (random y))
+    (if (= (random 2) 1)
+        (setq y-num (* y-num -1)))
+    (setq theta-num (random theta))
+    (if (= (random 2) 1)
+        (setq theta-num (* theta-num -1)))
+    (send ans-cds :translate (float-vector x-num y-num 0) :world)
+    (send ans-cds :rotate (deg2rad theta-num) :z :world)
+    ans-cds))
+
+(defun make-random-cds-list (initial-cds &key (x 100) (y 50) (theta 30) (num 10)) ;; TODO？指定した範囲の境界部分，最大最小？四隅？とかは絶対に入るようにする？回転はどうしようか．
+  (let (ans-cds-list list)
+    (dotimes (i num)
+      (setq ans-cds-list (append ans-cds-list (list (random-coords initial-cds :x x :y y :theta theta))))
+      )
+    ans-cds-list))
+
+(defun check ()
+  (let (pr2-spot-list (all-ik-success t) (num-ik-fail 0) (num-ik-fail-spot 0) (result-list (list)))
+    (setq pr2-spot-list (make-random-cds-list (make-coords :pos #f(1050 8500 30000) :rpy (float-vector pi 0 0)) :x 50 :y 50 :theta 10 :num 30)) ;; 前後左右50mm，回転±10degの範囲内でランダムに30箇所でチェック（今は範囲は適当）
+    ;; (setq pr2-spot-list (list (make-coords :pos #f(1050 8500 30000) :rpy (float-vector pi 0 0)) (make-coords :pos (float-vector 1200 8200 30000) :rpy (float-vector pi 0 0)))) ;; listで直接spotを指定，現在棚を操作している位置？とarrange-spotの座標．
+    (dolist (spot pr2-spot-list)
+      (send *pr2* :move-to (send spot :copy-worldcoords) :world)
+      (set-ladle);; move-rarm-to-ladle-at-shelfではladleが手にassocされていることが必要．
+      (setq result (move-rarm-to-ladle-at-shelf :send-ri nil));; move-rarm-to-ladle-at-shelfをriに送らずに実行．
+      (setq result-list (append result-list (list result)))
+      (format t "~A ik fail at ~A ~%" result spot)
+      )
+    (warn "---- IK check result ----- ~%")
+    (dotimes (i (length pr2-spot-list));; IK失敗時はログが出て埋もれるのでdolist内の個別spotの結果を全IK終了後にも描画．
+      (format t "~A ik fail at ~A ~%" (elt result-list i) (elt pr2-spot-list i))
+      (unless (= (elt result-list i) 0)
+        (setq all-ik-success nil)
+        (setq num-ik-fail (+ num-ik-fail (elt result-list i)))
+        (setq num-ik-fail-spot (+ num-ik-fail-spot 1))
+        )
+      )
+    (if all-ik-success
+        (format t "(total) all ik success!! ~%")
+      (format t "(total) ~A ik fail at ~A/~A spots ~%" num-ik-fail num-ik-fail-spot (length pr2-spot-list)))
+    ))

jsk_2021_10_soup_from_boil/euslisp/motion/tool-at-arrange.l

@@ -412,72 +412,92 @@
         ))
   )
 
-(defun move-rarm-to-ladle-at-shelf (&key (reverse nil))
+(defun move-rarm-to-ladle-at-shelf (&key (reverse nil) (send-ri t))
   ;; ladleを置く位置
   ;; 中間姿勢を多めに入れないと、IKがすぐに失敗する
-  (let (av0 av1 av2 av3 av4 ik-target av avs tms)
+  (let (av0 av1 av2 av3 av4 ik-target av avs tms ik-result (ik-fail-cnt 0))
     ;; Pre pose
     (setq ik-target (make-coords :pos (float-vector 570 8500 31100) :rpy (float-vector 3.14 0.0 0.0)))
     (send *pr2* :reset-manip-pose)
     (send *pr2* :torso :waist-z :joint-angle 325)
     (send *pr2* :head :look-at (send *shelf-look-coords* :worldpos))
-    (setq av0 (send *pr2* :rarm :inverse-kinematics
+    (setq ik-result (send *pr2* :rarm :inverse-kinematics
                     (send ik-target :copy-worldcoords)
                     :rotation-axis :z
                     :debug-view *debug-flag*))
+    (if (equal ik-result nil)
+        (setq ik-fail-cnt (+ ik-fail-cnt 1)))
+    (setq av0 (send *pr2* :angle-vector))
     ;; Set av1 ~ av4 おたまの先端を棚に入れるための姿勢
     (send (send *pr2* :rarm :end-coords) :dissoc *ladle*)
     (send *ladle* :newcoords (send *pr2* :rarm :end-coords :copy-worldcoords))
     (send (send *pr2* :rarm :end-coords) :assoc *ladle*)
     (setq ik-target (send *ladle-coords-at-shelf* :copy-worldcoords))
     (send ik-target :rotate (deg2rad 45) :z)
     (send ik-target :rotate (deg2rad 45) :y)
-    (setq av1 (send *pr2* :rarm :inverse-kinematics
+    (setq ik-result (send *pr2* :rarm :inverse-kinematics
                     (send ik-target :translate #f(200 0 50) :world)
                     :move-target (car (send *ladle* :attention))
                     :rotation-axis :t
                     :debug-view *debug-flag*))
+    (if (equal ik-result nil)
+        (setq ik-fail-cnt (+ ik-fail-cnt 1)))
+    (setq av1 (send *pr2* :angle-vector))
 
     (send ik-target :rotate (deg2rad -30) :y)
     (send ik-target :rotate (deg2rad -30) :z)
     (send ik-target :translate #f(-50 0 50) :world)
-    (setq av2 (send *pr2* :rarm :inverse-kinematics
+    (setq ik-result (send *pr2* :rarm :inverse-kinematics
                     ik-target
                     :move-target (car (send *ladle* :attention))
                     :rotation-axis :z
                     :debug-view *debug-flag*))
+    (if (equal ik-result nil)
+        (setq ik-fail-cnt (+ ik-fail-cnt 1)))
+    (setq av2 (send *pr2* :angle-vector))
     (setq ik-target (send *ladle-coords-at-shelf* :copy-worldcoords))
     (send ik-target :rotate (deg2rad -90) :x)
     (send ik-target :translate #f(200 70 30) :world)
     (send ik-target :rotate (deg2rad -20) :z)
-    (setq av3 (send *pr2* :rarm :inverse-kinematics
+    (setq ik-result (send *pr2* :rarm :inverse-kinematics
                     (send ik-target :copy-worldcoords)
                     :rotation-axis t
                     :debug-view *debug-flag*))
+    (if (equal ik-result nil)
+        (setq ik-fail-cnt (+ ik-fail-cnt 1)))
+    (setq av3 (send *pr2* :angle-vector))
     (send ik-target :rotate (deg2rad 20) :z)
     (send ik-target :translate #f(0 30 -10) :world)
-    (setq av4 (send *pr2* :rarm :inverse-kinematics
+    (setq ik-result (send *pr2* :rarm :inverse-kinematics
                     (send ik-target :copy-worldcoords)
                     :rotation-axis t
                     :debug-view *debug-flag*))
+    (if (equal ik-result nil)
+        (setq ik-fail-cnt (+ ik-fail-cnt 1)))
+    (setq av4 (send *pr2* :angle-vector))
     (send *viewer* :draw-objects)
     (setq avs (list av0 av1 av2 av3 av4))
     (setq tms (list 2000 2000 2000 2000 1000))
     ;; 手を棚の奥にスライドさせる
     (dotimes (i 25)
       (send ik-target :translate (float-vector -10 0 0) :world)
-      (send *pr2* :rarm :inverse-kinematics
+      (setq ik-result (send *pr2* :rarm :inverse-kinematics
             ik-target
             :rotation-axis t
-            :debug-view nil)
+            :debug-view nil))
+      (if (equal ik-result nil)
+          (setq ik-fail-cnt (+ ik-fail-cnt 1)))
       (setq av (send *pr2* :angle-vector))
       (setq avs (append avs (list av)))
       (setq tms (append tms (list 400))))
     ;; 実機を動かす。:reverseのキーワード引数がtになると、動作を逆再生する。
-    (if reverse
-      (send *ri* :angle-vector-sequence (reverse avs) (reverse tms))
-      (send *ri* :angle-vector-sequence avs tms))
-    (send *ri* :wait-interpolation)))
+    (if send-ri
+        (progn
+          (if reverse
+              (send *ri* :angle-vector-sequence (reverse avs) (reverse tms))
+            (send *ri* :angle-vector-sequence avs tms))
+          (send *ri* :wait-interpolation)))
+    ik-fail-cnt))
 
 ;; おたまを棚の中に置く
 (defun put-ladle-at-shelf ()