inverse-kinematicsがたまに落ちる

[ishiguroJSK]

https://github.com/jsk-ros-pkg/jsk_demos/blob/4228bae6fd5784cc169a6d03d9c69319d1f5a38a/rwt_teleop/euslisp/pr2-realtime-ik.l
みたいなコードで周期的にinverse-kinematicsを呼んでいるのですが，稀に以下のようなエラーを吐いて止まってしまいます．
何となくIKが解けない時にnanが混ざるんじゃないかと疑っているのですが，
ありうるのでしょうか？

ちなみにeuslispでnanってどうやって生成したらいいんでしょうか？

Call Stack (max depth: 20):
  0: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args)
  1: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args)
  2: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args)
  3: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args)
  4: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args)
  5: at (send-super* :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args)
  6: at (cond (use-base (setq diff-pos-rot (concatenate float-vector (send start-coords :difference-position self) (send start-coords :difference-rotation self))) (send self :move-to start-coords :world) (with-append-root-joint (ll self link-list :joint-class omniwheel-joint :joint-args base-range) (send (caar ll) :joint :joint-angle (float-vector (elt diff-pos-rot 0) (elt diff-pos-rot 1) (rad2deg (elt diff-pos-rot 5)))) (send-super* :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (append (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005)) (list (car (send self :links)) (if (eq use-base t) 0.1 use-base))) additional-weight-list) :link-list ll :move-target move-target args))) (t (send-super* :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args)))
  7: at (let (diff-pos-rot) (unless move-arm (setq move-arm (send self :select-target-arm target-coords))) (unless move-target (if (consp move-arm) (setq move-target (mapcar #'(lambda (arm) (send self arm :end-coords)) move-arm)) (setq move-target (send self move-arm :end-coords)))) (unless link-list (setq link-list (if (consp target-coords) (mapcar #'(lambda (target) (let ((l target) move-arm) (while l (cond ((memq l (send self :larm)) (setq move-arm :larm)) ((memq l (send self :rarm)) (setq move-arm :rarm))) (setq l (send l :parent))) (send self :link-list (send target :parent) (unless use-torso (car (send self move-arm)))))) move-target) (send self :link-list (send move-target :parent) (unless use-torso (car (send self move-arm))))))) (cond (use-base (setq diff-pos-rot (concatenate float-vector (send start-coords :difference-position self) (send start-coords :difference-rotation self))) (send self :move-to start-coords :world) (with-append-root-joint (ll self link-list :joint-class omniwheel-joint :joint-args base-range) (send (caar ll) :joint :joint-angle (float-vector (elt diff-pos-rot 0) (elt diff-pos-rot 1) (rad2deg (elt diff-pos-rot 5)))) (send-super* :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (append (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005)) (list (car (send self :links)) (if (eq use-base t) 0.1 use-base))) additional-weight-list) :link-list ll :move-target move-target args))) (t (send-super* :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :movehttps://github.com/jsk-ros-pkg/jsk_demos/blob/4228bae6fd5784cc169a6d03d9c69319d1f5a38a/rwt_teleop/euslisp/pr2-realtime-ik.l-target move-target args))))
  8: at (send *robot* :inverse-kinematics (list larm-tgt rarm-tgt) :move-target (list (send *robot* :larm :end-coords) (send *robot* :rarm :end-coords)) :translation-axis (list t t) :rotation-axis (list t t) :use-torso nil :stop 2 :revert-if-fail nil :debug-view nil)
  9: at (while (ros::ok) (send *robot* :inverse-kinematics (list larm-tgt rarm-tgt) :move-target (list (send *robot* :larm :end-coords) (send *robot* :rarm :end-coords)) :translation-axis (list t t) :rotation-axis (list t t) :use-torso nil :stop 2 :revert-if-fail nil :debug-view nil) (send *robot* :head :neck-p :joint-angle (rad2deg (elt (car (rpy-angle (send head-tgt :rot))) 1))) (send *robot* :head :neck-y :joint-angle (rad2deg (elt (car (rpy-angle (send head-tgt :rot))) 0))) (send *ri* :angle-vector (send *robot* :angle-vector) 100) (send *irtviewer* :draw-objects :flush nil) (dolist (tgt (list larm-tgt rarm-tgt)) (send tgt :draw-on :flush t)) (ros::spin-once) (ros::sleep))
  10: at (run)
/opt/ros/indigo/share/euslisp/jskeus/eus/Linux64/bin/irteusgl roseus-error: number expected in (apply #'send-message self (class . super) :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args), exitting...

[knorth55]

予想ですが、どこかの数値が来るべきArgumentにnilが来ていると思います

[YoheiKakiuchi]

irteusでは、
*nan* がnanを表して、
(c-isnan *nan*) でnan チェックできます

[ishiguroJSK]

今日わかったこととしては，IKの引数であるlarm-tgt, rarm-tgtをprintさせても，
nanやnilが入っているわけではなさそうで，
じっとしている時（目標手先位置を更新せずにIKを回している）にも落ちる．
わからない

#<coordinates #Xa63f080  841.838 443.531 685.789 / 0.0 1.571 0.0>
#<coordinates #Xc09da68  405.478 -287.258 1258.288 / 0.0 1.571 0.0>
[ WARN] [1608289349.856922807] [/pr2_realtime_ik:ros.roseus]: [head_traj_controller/follow_joint_trajectory] action-result-cb may recieved old client's goal
[ WARN] [1608289349.856978872] [/pr2_realtime_ik:ros.roseus]:      expected goal id 1608289349728491651_/pr2_realtime_ik_32020_head_traj_controller/follow_joint_trajectory_5401
[ WARN] [1608289349.857006786] [/pr2_realtime_ik:ros.roseus]:      recieved goal id 1608289349500970204_/pr2_realtime_ik_32020_head_traj_controller/follow_joint_trajectory_5400
[ WARN] [1608289349.857327406] [/pr2_realtime_ik:ros.roseus]: [torso_controller/follow_joint_trajectory] action-result-cb may recieved old client's goal
[ WARN] [1608289349.857368769] [/pr2_realtime_ik:ros.roseus]:      expected goal id 1608289349729078059_/pr2_realtime_ik_32020_torso_controller/follow_joint_trajectory_5401
[ WARN] [1608289349.857394016] [/pr2_realtime_ik:ros.roseus]:      recieved goal id 1608289349503138177_/pr2_realtime_ik_32020_torso_controller/follow_joint_trajectory_5400
[ WARN] [1608289349.890890075] [/pr2_realtime_ik:ros.roseus]: [l_arm_controller/follow_joint_trajectory] action-result-cb may recieved old client's goal
[ WARN] [1608289349.890923487] [/pr2_realtime_ik:ros.roseus]:      expected goal id 1608289349888574378_/pr2_realtime_ik_32020_l_arm_controller/follow_joint_trajectory_5402
[ WARN] [1608289349.890946291] [/pr2_realtime_ik:ros.roseus]:      recieved goal id 1608289349727554133_/pr2_realtime_ik_32020_l_arm_controller/follow_joint_trajectory_5401
[ WARN] [1608289349.895607678] [/pr2_realtime_ik:ros.roseus]: [r_arm_controller/follow_joint_trajectory] action-result-cb may recieved old client's goal
[ WARN] [1608289349.895644608] [/pr2_realtime_ik:ros.roseus]:      expected goal id 1608289349888977117_/pr2_realtime_ik_32020_r_arm_controller/follow_joint_trajectory_5402
[ WARN] [1608289349.895669054] [/pr2_realtime_ik:ros.roseus]:      recieved goal id 1608289349727912133_/pr2_realtime_ik_32020_r_arm_controller/follow_joint_trajectory_5401
#<coordinates #Xa63f080  841.838 443.531 685.789 / 0.0 1.571 0.0>
#<coordinates #Xc09da68  405.478 -287.258 1258.288 / 0.0 1.571 0.0>
[ WARN] [1608289350.055144751] [/pr2_realtime_ik:ros.roseus]: [head_traj_controller/follow_joint_trajectory] action-result-cb may recieved old client's goal
[ WARN] [1608289350.055195360] [/pr2_realtime_ik:ros.roseus]:      expected goal id 1608289349892380591_/pr2_realtime_ik_32020_head_traj_controller/follow_joint_trajectory_5402
[ WARN] [1608289350.055212863] [/pr2_realtime_ik:ros.roseus]:      recieved goal id 1608289349728491651_/pr2_realtime_ik_32020_head_traj_controller/follow_joint_trajectory_5401
[ WARN] [1608289350.058995910] [/pr2_realtime_ik:ros.roseus]: [torso_controller/follow_joint_trajectory] action-result-cb may recieved old client's goal
[ WARN] [1608289350.059037499] [/pr2_realtime_ik:ros.roseus]:      expected goal id 1608289349901272685_/pr2_realtime_ik_32020_torso_controller/follow_joint_trajectory_5402
[ WARN] [1608289350.059054199] [/pr2_realtime_ik:ros.roseus]:      recieved goal id 1608289349729078059_/pr2_realtime_ik_32020_torso_controller/follow_joint_trajectory_5401
[ WARN] [1608289350.081761989] [/pr2_realtime_ik:ros.roseus]: [l_arm_controller/follow_joint_trajectory] action-result-cb may recieved old client's goal
[ WARN] [1608289350.081796407] [/pr2_realtime_ik:ros.roseus]:      expected goal id 1608289350079566578_/pr2_realtime_ik_32020_l_arm_controller/follow_joint_trajectory_5403
[ WARN] [1608289350.081816961] [/pr2_realtime_ik:ros.roseus]:      recieved goal id 1608289349888574378_/pr2_realtime_ik_32020_l_arm_controller/follow_joint_trajectory_5402
[ WARN] [1608289350.083226942] [/pr2_realtime_ik:ros.roseus]: [r_arm_controller/follow_joint_trajectory] action-result-cb may recieved old client's goal
[ WARN] [1608289350.083249021] [/pr2_realtime_ik:ros.roseus]:      expected goal id 1608289350079908391_/pr2_realtime_ik_32020_r_arm_controller/follow_joint_trajectory_5403
[ WARN] [1608289350.083265288] [/pr2_realtime_ik:ros.roseus]:      recieved goal id 1608289349888977117_/pr2_realtime_ik_32020_r_arm_controller/follow_joint_trajectory_5402
#<coordinates #Xa63f080  841.838 443.531 685.789 / 0.0 1.571 0.0>
#<coordinates #Xc09da68  405.478 -287.258 1258.288 / 0.0 1.571 0.0>
[ WARN] [1608289350.255222115] [/pr2_realtime_ik:ros.roseus]: [head_traj_controller/follow_joint_trajectory] action-result-cb may recieved old client's goal
[ WARN] [1608289350.255264226] [/pr2_realtime_ik:ros.roseus]:      expected goal id 1608289350080200593_/pr2_realtime_ik_32020_head_traj_controller/follow_joint_trajectory_5403
[ WARN] [1608289350.255279688] [/pr2_realtime_ik:ros.roseus]:      recieved goal id 1608289349892380591_/pr2_realtime_ik_32020_head_traj_controller/follow_joint_trajectory_5402
[ WARN] [1608289350.258381892] [/pr2_realtime_ik:ros.roseus]: [torso_controller/follow_joint_trajectory] action-result-cb may recieved old client's goal
[ WARN] [1608289350.258424324] [/pr2_realtime_ik:ros.roseus]:      expected goal id 1608289350083409737_/pr2_realtime_ik_32020_torso_controller/follow_joint_trajectory_5403
[ WARN] [1608289350.258440425] [/pr2_realtime_ik:ros.roseus]:      recieved goal id 1608289349901272685_/pr2_realtime_ik_32020_torso_controller/follow_joint_trajectory_5402
[ WARN] [1608289350.292870921] [/pr2_realtime_ik:ros.roseus]: [l_arm_controller/follow_joint_trajectory] action-result-cb may recieved old client's goal
[ WARN] [1608289350.292905747] [/pr2_realtime_ik:ros.roseus]:      expected goal id 1608289350289779090_/pr2_realtime_ik_32020_l_arm_controller/follow_joint_trajectory_5404
[ WARN] [1608289350.292920657] [/pr2_realtime_ik:ros.roseus]:      recieved goal id 1608289350079566578_/pr2_realtime_ik_32020_l_arm_controller/follow_joint_trajectory_5403
[ WARN] [1608289350.298930378] [/pr2_realtime_ik:ros.roseus]: [r_arm_controller/follow_joint_trajectory] action-result-cb may recieved old client's goal
[ WARN] [1608289350.298965295] [/pr2_realtime_ik:ros.roseus]:      expected goal id 1608289350290132644_/pr2_realtime_ik_32020_r_arm_controller/follow_joint_trajectory_5404
[ WARN] [1608289350.298983111] [/pr2_realtime_ik:ros.roseus]:      recieved goal id 1608289350079908391_/pr2_realtime_ik_32020_r_arm_controller/follow_joint_trajectory_5403
#<coordinates #Xa63f080  841.838 443.531 685.789 / 0.0 1.571 0.0>
#<coordinates #Xc09da68  405.478 -287.258 1258.288 / 0.0 1.571 0.0>
Call Stack (max depth: 20):
  0: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args)
  1: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args)
  2: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args)
  3: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args)
  4: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args)
  5: at (send-super* :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args)
  6: at (cond (use-base (setq diff-pos-rot (concatenate float-vector (send start-coords :difference-position self) (send start-coords :difference-rotation self))) (send self :move-to start-coords :world) (with-append-root-joint (ll self link-list :joint-class omniwheel-joint :joint-args base-range) (send (caar ll) :joint :joint-angle (float-vector (elt diff-pos-rot 0) (elt diff-pos-rot 1) (rad2deg (elt diff-pos-rot 5)))) (send-super* :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (append (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005)) (list (car (send self :links)) (if (eq use-base t) 0.1 use-base))) additional-weight-list) :link-list ll :move-target move-target args))) (t (send-super* :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args)))
  7: at (let (diff-pos-rot) (unless move-arm (setq move-arm (send self :select-target-arm target-coords))) (unless move-target (if (consp move-arm) (setq move-target (mapcar #'(lambda (arm) (send self arm :end-coords)) move-arm)) (setq move-target (send self move-arm :end-coords)))) (unless link-list (setq link-list (if (consp target-coords) (mapcar #'(lambda (target) (let ((l target) move-arm) (while l (cond ((memq l (send self :larm)) (setq move-arm :larm)) ((memq l (send self :rarm)) (setq move-arm :rarm))) (setq l (send l :parent))) (send self :link-list (send target :parent) (unless use-torso (car (send self move-arm)))))) move-target) (send self :link-list (send move-target :parent) (unless use-torso (car (send self move-arm))))))) (cond (use-base (setq diff-pos-rot (concatenate float-vector (send start-coords :difference-position self) (send start-coords :difference-rotation self))) (send self :move-to start-coords :world) (with-append-root-joint (ll self link-list :joint-class omniwheel-joint :joint-args base-range) (send (caar ll) :joint :joint-angle (float-vector (elt diff-pos-rot 0) (elt diff-pos-rot 1) (rad2deg (elt diff-pos-rot 5)))) (send-super* :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (append (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005)) (list (car (send self :links)) (if (eq use-base t) 0.1 use-base))) additional-weight-list) :link-list ll :move-target move-target args))) (t (send-super* :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args))))
  8: at (send *robot* :inverse-kinematics (list larm-tgt rarm-tgt) :move-target (list (send *robot* :larm :end-coords) (send *robot* :rarm :end-coords)) :translation-axis (list t t) :rotation-axis (list t t) :use-torso nil :stop 2 :revert-if-fail nil :debug-view nil)
  9: at (while (ros::ok) (print larm-tgt) (print rarm-tgt) (send *robot* :inverse-kinematics (list larm-tgt rarm-tgt) :move-target (list (send *robot* :larm :end-coords) (send *robot* :rarm :end-coords)) :translation-axis (list t t) :rotation-axis (list t t) :use-torso nil :stop 2 :revert-if-fail nil :debug-view nil) (send *robot* :head :neck-p :joint-angle (rad2deg (elt (car (rpy-angle (send head-tgt :rot))) 1))) (send *robot* :head :neck-y :joint-angle (rad2deg (elt (car (rpy-angle (send head-tgt :rot))) 0))) (send *ri* :angle-vector (send *robot* :angle-vector) 100) (send *irtviewer* :draw-objects :flush nil) (dolist (tgt (list larm-tgt rarm-tgt)) (send tgt :draw-on :flush t)) (ros::spin-once) (ros::sleep))
  10: at (run)
/opt/ros/indigo/share/euslisp/jskeus/eus/Linux64/bin/irteusgl roseus-error: number expected in (apply #'send-message self (class . super) :inverse-kinematics target-coords :rthre rthre :thre thre :stop stop :additional-weight-list (list (list (send self :torso_lift_joint :child-link) (if (numberp use-torso) use-torso 0.005))) :link-list link-list :move-target move-target args), exitting...
[ INFO] [1608289350.476730853] [/pr2_realtime_ik:ros.roseus]: cell* ROSEUS_EXIT(context*, int, cell**)
[ INFO] [1608289350.476755125] [/pr2_realtime_ik:ros.roseus]: exiting roseus 1
[pr2_realtime_ik-6] process has died [pid 32020, exit code 1, cmd /home/applications/ros/indigo/src/jsk-ros-pkg/jsk_demos/rwt_teleop/euslisp/pr2-realtime-ik.l __name:=pr2_realtime_ik __log:=/home/applications/.ros/log/7e3dc4dc-4115-11eb-8acd-d05099c29feb/pr2_realtime_ik-6.log].
log file: /home/applications/.ros/log/7e3dc4dc-4115-11eb-8acd-d05099c29feb/pr2_realtime_ik-6*.log

[Naoki-Hiraoka]

load "package://euslisp/jskeus/irteus/irtgeo.l"
load "package://euslisp/jskeus/irteus/irtmodel.l"

をプログラムの冒頭に加えると、プログラムの動作速度は低下しますが、エラーメッセージがより詳しくなるのでどこで落ちているのかわかりやすくなるのではないかと思います。

[ishiguroJSK]

教えてもらったコードも入れて何度か長時間回してるけど，再現しなくなってしまった…
実機のモータドライバの通信不良も要因だったんじゃないかとも思えてきたけど，
最近PR1040通信不良が少ないので分からない…

[Affonso-Gui]

.l をロードすることでcallstackがより詳しくなりデバッグしやすいが、.lの挙動と.soの挙動は基本的に違うので、.lで再現しなくなる可能性もあります。
もしコンパイルコードでまた再現性がとれるようでしたら、事前にroseus --gdbや事後にgdb$ attach <PID> で詳しいデバッグができます。