When execute pr2' /tmp/irtmodel-ik faiuler file, I get error "error: unbound variable l_gripper_tool_frame"

[aginika]

When I execute the below code (which is the program that is removed the "additionals"),

$ cat pr2-sensor-robot-2014-07-12-17-44-47-failure.l
;; ik success log at Sat Jul 12 17:44:47 2014 on (9.00 inagaki Sat Apr 12 18:33:24 JST 2014 fcc6e04 0140229)
;;
;; link-list (#<bodyset-link #Xe19d058 l_shoulder_pan_link  3081.927 1401.466 840.675 / 2.89 -2.300e-17 9.861e-17> #<bodyset-link #Xe1fe180 l_shoulder_lift_link  2985.078 1426.373 840.675 / 2.89 -0.353 1.249e-16> #<bodyset-link #Xe048400 l_upper_arm_roll_link  2985.078 1426.373 840.675 / 2.89 -0.353 1.266> #<bodyset-link #Xe1161d0 l_elbow_flex_link  2621.631 1519.842 979.13 / 0.856 -0.135 2.015> #<bodyset-link #Xdaf5cd0 l_forearm_roll_link  2621.631 1519.842 979.13 / 0.856 -0.135 -1.724> #<bodyset-link #Xde01bc0 l_wrist_flex_link  2830.219 1760.003 1022.2 / 1.571 2.940e-07 -1.774> #<bodyset-link #Xdef57b0 l_wrist_roll_link  2830.219 1760.003 1022.2 / 1.571 2.940e-07 -1.779e-09>)
;; move-target #<cascaded-coords #Xe7f8570 :larm-end-coords  2830.219 1940.003 1022.2 / 1.571 2.940e-07 -1.779e-09>
;; rotatoin-axis t, translation-axis t
;; thre 10, rthre 0.087266, stop 300
(setq c0 '(#s(coordinates plist nil rot #2f((-2.829885e-08 -1.0 -1.779092e-09) (1.0 -2.829885e-08 2.940477e-07) (-2.940477e-07 -1.779084e-09 1.0)) pos #f(2830.22 1840.0 1022.2))))
(setq av0 #f(50.0 75.5776 -20.2513 72.5163 -111.664 145.774 -41.5684 101.667 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 38.3634 28.5049))
(defun pr2-sensor-robot-2014-07-12-17-44-47-setup () (let ((r (instance pr2-sensor-robot :init))) (setq *robot* r) (progn (send r :newcoords (make-coords :4x4 #2f((6.106227e-16 -1.0 4.683753e-17 3269.93) (1.0 6.106227e-16 8.977194e-17 1451.47) (-8.977194e-17 4.683753e-17 1.0 0.0) (0.0 0.0 0.0 1.0)))) (mapc #'(lambda (j a) (send* j :joint-angle a nil)) (list (send r :torso_lift_joint) (send r :l_shoulder_pan_joint) (send r :l_shoulder_lift_joint) (send r :l_upper_arm_roll_joint) (send r :l_elbow_flex_joint) (send r :l_forearm_roll_joint) (send r :l_wrist_flex_joint) (send r :l_wrist_roll_joint) (send r :r_shoulder_pan_joint) (send r :r_shoulder_lift_joint) (send r :r_upper_arm_roll_joint) (send r :r_elbow_flex_joint) (send r :r_forearm_roll_joint) (send r :r_wrist_flex_joint) (send r :r_wrist_roll_joint) (send r :head_pan_joint) (send r :head_tilt_joint)) '(50.0 75.5776 -20.2513 72.5163 -111.664 145.774 -41.5684 101.667 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 38.3634 28.5049)) (objects (list r))) (objects (list *robot*))))
(defun pr2-sensor-robot-2014-07-12-17-44-47-check () (let ((r *robot*)) (send* r :inverse-kinematics (list (make-coords :pos #f(2830.22 1840.0 1022.2) :rot #2f((-2.829885e-08 -1.0 -1.779092e-09) (1.0 -2.829885e-08 2.940477e-07) (-2.940477e-07 -1.779084e-09 1.0))) :dump-command nil :debug-view t :move-target (let* ((p (send r l_gripper_tool_frame)) (c (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-cascoords :4x4 #2f((1.0 0.0 0.0 0.0) (0.0 1.0 0.0 0.0) (0.0 0.0 1.0 0.0) (0.0 0.0 0.0 1.0)))) :parent p))) c) :link-list (list (send r l_shoulder_pan_link) (send r l_shoulder_lift_link) (send r l_upper_arm_roll_link) (send r l_elbow_flex_link) (send r l_forearm_roll_link) (send r l_wrist_flex_link) (send r l_wrist_roll_link)) :rthre 0.087266 :thre 10 :stop 300 :link-list (list (send r l_shoulder_pan_link) (send r l_shoulder_lift_link) (send r l_upper_arm_roll_link) (send r l_elbow_flex_link) (send r l_forearm_roll_link) (send r l_wrist_flex_link) (send r l_wrist_roll_link)) :move-target (let* ((p (send r l_gripper_tool_frame)) (c (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-cascoords :4x4 #2f((1.0 0.0 0.0 0.0) (0.0 1.0 0.0 0.0) (0.0 0.0 1.0 0.0) (0.0 0.0 0.0 1.0)))) :parent p))) c) :move-target (let* ((p (send r l_gripper_tool_frame)) (c (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-cascoords :4x4 #2f((1.0 0.0 0.0 0.0) (0.0 1.0 0.0 0.0) (0.0 0.0 1.0 0.0) (0.0 0.0 0.0 1.0)))) :parent p))) c) :collision-avoidance-link-pair nil :link-list (list (send r l_shoulder_pan_link) (send r l_shoulder_lift_link) (send r l_upper_arm_roll_link) (send r l_elbow_flex_link) (send r l_forearm_roll_link) (send r l_wrist_flex_link) (send r l_wrist_roll_link))))))
(defun ik-setup () (pr2-sensor-robot-2014-07-12-17-44-47-setup))
(defun ik-check () (pr2-sensor-robot-2014-07-12-17-44-47-check))
(setq ik-failed '((:dif-pos . (#f(-7.99891 -9.70584 -7.17859))) (:dif-rot . (#f(0.004874 0.0015 -0.006467))
)))

When I do below step

roseus pr2-sensor-robot-2014-07-12-17-44-47-failure.l
(load "package://pr2eus/pr2-interface.l")
(ik-setup)
(ik-check) ;;FAIL HERE

I get below lines

1.irteusgl$ load "package://pr2eus/pr2-interface.l"
t
2.irteusgl$ ik-setup
;; (make-irtviewer) executed
(#<pr2-sensor-robot #X73af670 pr2  3269.93 1451.47 0.0 / 1.571 8.977e-17 4.684e-17>)
3.irteusgl$ ik-check
/home/inagaki/ros/hydro/src/jsk-ros-pkg/jsk_roseus/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: unbound variable l_gripper_tool_frame in (send r l_gripper_tool_frame)

What should I do?

[k-okada]

(send r l_gripper_tool_frame)

it seems forget to add : when writing *-failure.l code.
check if (send r :l_gripper_tool_frame) works and if so, check source code around https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L2068

[aginika]

The error seems to be come from 2 reason.

1. the :name return value is invalid. l_gripper_tool_frame should be l_gripper_tool_frame_lk in pr2.l
2. As you said, the : is missing.

As for 1. , I attached the patch to pr2eus/pr2.l to add "_lk" to all ":name" value and it seems to be repair.
However , the file said that file shouldn't be edited and I am not sure that it would be no problem to change it or not...
@snozawa Is there any reason the:name value is remained to be without "_lk"?

As for 2., the variable print-args seems to have the wrong string and that is created around https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L2048.
I edited all (send r ,(send x :name)) to (send r :,(send x :name)), but when I executed the motion which will fail(I run setup.l in jsk_2013_04_pr2_610/euslisp) the output failuer file wasn't changed.
So I mean, the (send r ,(send x :name)) was still (send r ,(send x :name)) after I edit irtmodel.l.
@k-okada Do I forget something to do to reflect my change?

[snozawa]

@snozawa Is there any reason the:name value is remained to be without "_lk"?

I created this PR to use :link method for access and add test code:
euslisp/jskeus#101

I need to another PR to fix this problem completely.

[snozawa]

I fixed dump of ik fail log.

• Dump links as loadable format like (send r :link link-name)[1]
• Fix move-target to use links included in (send r :links)[2]
  l_gripper_tool_frame is virtual link and not included in (send r :links).
  So, even if we dump the link as (send r :l_gripper_tool_frame), it returns nil and ik fail log does not work.
• Escape link and string to dump list and string properly (It would be better to use dump-lodable-structure??)
  (xxx) => (list xxx)
"aaa" => "\"aaa\""

I also add test codes to test/test-irt-motion.l through [1] and [2].

[1] euslisp/jskeus#101
[2] euslisp/jskeus#103