/tmp/irtmodel-ik failuer file will become segmentation fault when I execute with roseus #138

aginika · 2014-07-12T09:14:31Z

When I try to execute kind of below code with roseus command, it become segmentation fault. Ths success one doesn't fail.

Is it because of my environment?(it failed ,though I tried in other robot environment too)

failuer sample

$ 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 :additional-weight-list ((#<bodyset-link #Xe6e4df0 torso_lift_link  3269.927 1401.466 840.675 / 1.571 8.977e-17 4.684e-17> 0.005)) :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 execute roseus

connected to Xserver DISPLAY=:0.0
X events are being asynchronously monitored.
;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtpointcloud irtx eusjpeg euspng png irtimage irtglrgb 
;; extending gcstack 0x61ba9a0[16374] --> 0x661f390[32748] top=3767
irtgl irtviewer 
EusLisp 9.00(fcc6e04 0140229) for Linux64 created on inagaki(Sat Apr 12 18:33:24 JST 2014)
roseus ;; loading roseus("f8bf8b3") on euslisp((9.00 inagaki Sat Apr 12 18:33:24 JST 2014 fcc6e04 0140229))
eustf roseus_c_util ;; Segmentation Fault.
;; in #<compiled-code #X61db528>
;; You are still in a signal handler.
;;Try reset or throw to upper level as soon as possible.
;; code=-636959504 x=da08c3c0 addr=11b
Fatal:

The text was updated successfully, but these errors were encountered:

k-okada · 2014-07-13T06:53:40Z

please remove :additional-weight-list ((#<bodyset-link #Xe6e4df0 torso_lift_link ...)) from pr2-sensor-robot-2014-07-12-17-44-47-failure.l and see if this works.

aginika · 2014-07-13T07:03:25Z

Removing surpress the segmentation fault! Thanks.

k-okada · 2014-07-13T07:09:57Z

ok, it is not end, we just find starting point ;-)

the problem is #<bodyset-link #Xe6e4df0 torso_lift_link ... is the pointer to the object and #X.. is the memory location so that is not valid once you restart the euslisp environment.

so please check

if you just remove :additional-wait-list .. then it produces different solution
replace #<body-link .... torso_lift_link> with something like (send r :torso_lift_link) then it generate same solution.

aginika · 2014-07-13T07:25:27Z

Hmm... I couldn't understand what should I do...

I did 1) at previous my comment. but what is "different solution" you meand and what to check?

At this time , I replase #<body> with (send r :torso_lift_link) and this suppress the segmentation too..

k-okada · 2014-07-13T07:33:24Z

I did 1) at previous my comment. but what is "different solution" you meand and what to check?

when pr2-sensor-robot-2014-07-12-17-44-47-failure.l is produced you saw some error message (ik-failed...) and you can see how close the end-coords at the end of iteration in distance in xyz and rpy (dif-pos and dif-rot in [1]).

and if you remove :additional-weight-list and execute "-filufre.l" then you will see ik fail error message with different distance or it may success, because it uses different ik parameters than when euslisp generated "-failure.l" generated.

[1] https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L2034

aginika · 2014-07-13T07:53:49Z

OK. I may understand.
Finally I will need compare the dif-pos and dif-rot among message which is in terminal , message which is produced by removed program and message which is produced by replaced program.

BTW, I get some error when I did ik_setup and ik_check, so I will open new issue.
After that is cleared, I will check this issue.

aginika · 2014-07-13T11:16:09Z

The diff results were as below.

Original is the output when motion file is played.
Remove additional is the output when remove :additional-weight-list.
Repair additional is the output when replace #<bodyset-link with (send r :torso_lift_link_lk).

2.and 3. was same, so the replace didn't seem to affect.

Below is Summary

;;original result=================================
;;ik-failed
;;((:dif-pos #f(-30.7121 -6.19955 -7.29337)) (:dif-rot #f(-0.009168 -0.004614 0.043459)))
dif-pos #f(-30.7121 -6.19955 -7.29337)
dif-rot #f(-0.009168 -0.004614 0.043459)

;;remove additional=================================
;; inverse-kinematics failed.
dif-pos : #f(-30.7084 -6.19854 -7.2926)/(32.1653/10)
dif-rot : #f(-0.009167 -0.004614 0.043453)/(0.044649/0.087266)


;;repair additional=================================    
dif-pos : #f(-30.7084 -6.19854 -7.2926)/(32.1653/10)
dif-rot : #f(-0.009167 -0.004614 0.043453)/(0.044649/0.087266)

Below is detail output

Original

;; inverse-kinematics failed.
;; dif-pos : #f(-30.7121 -6.19955 -7.29337)/(32.1692/10)
;; dif-rot : #f(-0.009168 -0.004614 0.043459)/(0.044654/0.087266)
;;  coords : #<coordinates #Xbf31ba0  3326.358 1349.163 -0.854 / 1.544 0.004 0.002>
;;  angles : (50.0 110.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -15.5326 32.7327 -79.4761 -121.542 -179.057 -106.754 250.856 -1.12702 58.7871)
;;    args : ((#<coordinates #Xe6d3c20  3351.999 1805.369 684.995 / 1.571 0.524 -1.829e-06>) :move-target #<cascaded-coords #Xe34c110 :rarm-end-coords  3347.207 1835.803 675.74 / 1.521 0.528 -0.016> :link-list (#<bodyset-link #Xe0cfd70 r_shoulder_pan_link_lk  3514.721 1297.459 839.637 / 1.293 0.004 0.001> #<bodyset-link #Xe130fd0 r_shoulder_lift_link_lk  3542.142 1393.625 839.199 / 1.293 0.32 0.001> #<bodyset-link #Xe19a738 r_upper_arm_roll_link_lk  3542.142 1393.625 839.199 / 1.293 0.32 -1.57> #<bodyset-link #Xe048ee8 r_elbow_flex_link_lk  3646.142 1758.79 713.349 / -2.892 -0.166 -1.296> #<bodyset-link #Xe096000 r_forearm_roll_link_lk  3646.142 1758.79 713.349 / -2.892 -0.166 2.074> #<bodyset-link #Xdf13ce8 r_wrist_flex_link_lk  3339.39 1680.5 766.404 / 1.521 0.528 1.566> #<bodyset-link #Xdf5fa20 r_wrist_roll_link_lk  3339.39 1680.5 766.404 / 1.521 0.528 -0.016>) :rthre 0.087266 :thre 10 :stop 300 :additional-weight-list ((#<bodyset-link #Xe266e18 torso_lift_link_lk  3326.789 1302.498 840.013 / 1.544 0.004 0.002> 0.005)) :link-list (#<bodyset-link #Xe0cfd70 r_shoulder_pan_link_lk  3514.721 1297.459 839.637 / 1.293 0.004 0.001> #<bodyset-link #Xe130fd0 r_shoulder_lift_link_lk  3542.142 1393.625 839.199 / 1.293 0.32 0.001> #<bodyset-link #Xe19a738 r_upper_arm_roll_link_lk  3542.142 1393.625 839.199 / 1.293 0.32 -1.57> #<bodyset-link #Xe048ee8 r_elbow_flex_link_lk  3646.142 1758.79 713.349 / -2.892 -0.166 -1.296> #<bodyset-link #Xe096000 r_forearm_roll_link_lk  3646.142 1758.79 713.349 / -2.892 -0.166 2.074> #<bodyset-link #Xdf13ce8 r_wrist_flex_link_lk  3339.39 1680.5 766.404 / 1.521 0.528 1.566> #<bodyset-link #Xdf5fa20 r_wrist_roll_link_lk  3339.39 1680.5 766.404 / 1.521 0.528 -0.016>) :move-target #<cascaded-coords #Xe34c110 :rarm-end-coords  3347.207 1835.803 675.74 / 1.521 0.528 -0.016> :move-target #<cascaded-coords #Xe34c110 :rarm-end-coords  3347.207 1835.803 675.74 / 1.521 0.528 -0.016> :collision-avoidance-link-pair nil :link-list (#<bodyset-link #Xe0cfd70 r_shoulder_pan_link_lk  3514.721 1297.459 839.637 / 1.293 0.004 0.001> #<bodyset-link #Xe130fd0 r_shoulder_lift_link_lk  3542.142 1393.625 839.199 / 1.293 0.32 0.001> #<bodyset-link #Xe19a738 r_upper_arm_roll_link_lk  3542.142 1393.625 839.199 / 1.293 0.32 -1.57> #<bodyset-link #Xe048ee8 r_elbow_flex_link_lk  3646.142 1758.79 713.349 / -2.892 -0.166 -1.296> #<bodyset-link #Xe096000 r_forearm_roll_link_lk  3646.142 1758.79 713.349 / -2.892 -0.166 2.074> #<bodyset-link #Xdf13ce8 r_wrist_flex_link_lk  3339.39 1680.5 766.404 / 1.521 0.528 1.566> #<bodyset-link #Xdf5fa20 r_wrist_roll_link_lk  3339.39 1680.5 766.404 / 1.521 0.528 -0.016>))
;; command : (let ((r (instance pr2-sensor-robot :init))) (progn (send r :newcoords (make-coords :4x4 #2f((0.026793 -0.999639 0.002106 3326.36) (0.999633 0.026801 0.003945 1349.16) (-0.004 0.002 0.99999 -0.854) (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 110.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -15.5326 32.7327 -79.4761 -121.542 -179.057 -106.754 250.856 -1.12702 58.7871)) (objects (list r))) (send* r :inverse-kinematics (list (make-coords :pos #f(3352.0 1805.37 684.995) :rot #2f((3.174644e-06 -1.0 3.960159e-09) (0.866025 2.751300e-06 0.500001) (-0.500001 -1.583895e-06 0.866025))) :dump-command nil :debug-view t :move-target (let* ((p (send r r_gripper_tool_frame_lk)) (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 r_shoulder_pan_link_lk) (send r r_shoulder_lift_link_lk) (send r r_upper_arm_roll_link_lk) (send r r_elbow_flex_link_lk) (send r r_forearm_roll_link_lk) (send r r_wrist_flex_link_lk) (send r r_wrist_roll_link_lk)) :rthre 0.087266 :thre 10 :stop 300 :additional-weight-list ((#<bodyset-link #Xe266e18 torso_lift_link_lk  3326.789 1302.498 840.013 / 1.544 0.004 0.002> 0.005)) :link-list (list (send r r_shoulder_pan_link_lk) (send r r_shoulder_lift_link_lk) (send r r_upper_arm_roll_link_lk) (send r r_elbow_flex_link_lk) (send r r_forearm_roll_link_lk) (send r r_wrist_flex_link_lk) (send r r_wrist_roll_link_lk)) :move-target (let* ((p (send r r_gripper_tool_frame_lk)) (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 r_gripper_tool_frame_lk)) (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 r_shoulder_pan_link_lk) (send r r_shoulder_lift_link_lk) (send r r_upper_arm_roll_link_lk) (send r r_elbow_flex_link_lk) (send r r_forearm_roll_link_lk) (send r r_wrist_flex_link_lk) (send r r_wrist_roll_link_lk)))))
;; dump debug command to /tmp/irtmodel-ik-11949/pr2-sensor-robot-2014-07-13-19-43-40-failure.l
;; (progn (load "/tmp/irtmodel-ik-11949/pr2-sensor-robot-2014-07-13-19-43-40-failure.l")(ik-setup)(ik-check))

Remove additional

;; inverse-kinematics failed.
;; dif-pos : #f(-30.7084 -6.19854 -7.2926)/(32.1653/10)
;; dif-rot : #f(-0.009167 -0.004614 0.043453)/(0.044649/0.087266)
;;  coords : #<coordinates #X7ee3428  3326.36 1349.16 -0.854 / 1.544 0.004 0.002>
;;  angles : (50.0 110.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -15.5326 32.7327 -79.4761 -121.542 -179.057 -106.754 250.856 -1.12702 58.7871)
;;    args : ((#<coordinates #X802c1b8  3352.0 1805.37 684.995 / 1.571 0.524 -1.829e-06>) :move-target #<cascaded-coords #X93fb0a0  3347.209 1835.8 675.741 / 1.521 0.528 -0.016> :link-list (#<bodyset-link #X7029268 r_shoulder_pan_link_lk  3514.723 1297.456 839.637 / 1.293 0.004 0.001> #<bodyset-link #X702cb98 r_shoulder_lift_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 0.001> #<bodyset-link #X702d1f8 r_upper_arm_roll_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 -1.57> #<bodyset-link #X7036150 r_elbow_flex_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 -1.296> #<bodyset-link #X7036678 r_forearm_roll_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 2.074> #<bodyset-link #X6f17890 r_wrist_flex_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 1.566> #<bodyset-link #X6f01ac0 r_wrist_roll_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 -0.016>) :rthre 0.087266 :thre 10 :stop 300 :additional-weight-list ((#<bodyset-link #X7028a10 torso_lift_link_lk  3326.791 1302.495 840.013 / 1.544 0.004 0.002> 0.005)) :link-list (#<bodyset-link #X7029268 r_shoulder_pan_link_lk  3514.723 1297.456 839.637 / 1.293 0.004 0.001> #<bodyset-link #X702cb98 r_shoulder_lift_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 0.001> #<bodyset-link #X702d1f8 r_upper_arm_roll_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 -1.57> #<bodyset-link #X7036150 r_elbow_flex_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 -1.296> #<bodyset-link #X7036678 r_forearm_roll_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 2.074> #<bodyset-link #X6f17890 r_wrist_flex_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 1.566> #<bodyset-link #X6f01ac0 r_wrist_roll_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 -0.016>) :move-target #<cascaded-coords #X93fb0a0  3347.209 1835.8 675.741 / 1.521 0.528 -0.016> :dump-command nil :debug-view t :move-target #<cascaded-coords #X93fb0a0  3347.209 1835.8 675.741 / 1.521 0.528 -0.016> :link-list (#<bodyset-link #X7029268 r_shoulder_pan_link_lk  3514.723 1297.456 839.637 / 1.293 0.004 0.001> #<bodyset-link #X702cb98 r_shoulder_lift_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 0.001> #<bodyset-link #X702d1f8 r_upper_arm_roll_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 -1.57> #<bodyset-link #X7036150 r_elbow_flex_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 -1.296> #<bodyset-link #X7036678 r_forearm_roll_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 2.074> #<bodyset-link #X6f17890 r_wrist_flex_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 1.566> #<bodyset-link #X6f01ac0 r_wrist_roll_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 -0.016>) :rthre 0.087266 :thre 10 :stop 300 :link-list (#<bodyset-link #X7029268 r_shoulder_pan_link_lk  3514.723 1297.456 839.637 / 1.293 0.004 0.001> #<bodyset-link #X702cb98 r_shoulder_lift_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 0.001> #<bodyset-link #X702d1f8 r_upper_arm_roll_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 -1.57> #<bodyset-link #X7036150 r_elbow_flex_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 -1.296> #<bodyset-link #X7036678 r_forearm_roll_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 2.074> #<bodyset-link #X6f17890 r_wrist_flex_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 1.566> #<bodyset-link #X6f01ac0 r_wrist_roll_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 -0.016>) :move-target #<cascaded-coords #X829c480  3347.209 1835.8 675.741 / 1.521 0.528 -0.016> :move-target #<cascaded-coords #X82ddbf0  3347.209 1835.8 675.741 / 1.521 0.528 -0.016> :collision-avoidance-link-pair nil :link-list (#<bodyset-link #X7029268 r_shoulder_pan_link_lk  3514.723 1297.456 839.637 / 1.293 0.004 0.001> #<bodyset-link #X702cb98 r_shoulder_lift_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 0.001> #<bodyset-link #X702d1f8 r_upper_arm_roll_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 -1.57> #<bodyset-link #X7036150 r_elbow_flex_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 -1.296> #<bodyset-link #X7036678 r_forearm_roll_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 2.074> #<bodyset-link #X6f17890 r_wrist_flex_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 1.566> #<bodyset-link #X6f01ac0 r_wrist_roll_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 -0.016>))

Repair additional

;; inverse-kinematics failed.
;; dif-pos : #f(-30.7084 -6.19854 -7.2926)/(32.1653/10)
;; dif-rot : #f(-0.009167 -0.004614 0.043453)/(0.044649/0.087266)
;;  coords : #<coordinates #X75ff368  3326.36 1349.16 -0.854 / 1.544 0.004 0.002>
;;  angles : (50.0 110.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -15.5326 32.7327 -79.4761 -121.542 -179.057 -106.754 250.856 -1.12702 58.7871)
;;    args : ((#<coordinates #X777ccf8  3352.0 1805.37 684.995 / 1.571 0.524 -1.829e-06>) :move-target #<cascaded-coords #X89da730  3347.209 1835.8 675.741 / 1.521 0.528 -0.016> :link-list (#<bodyset-link #X68a9970 r_shoulder_pan_link_lk  3514.723 1297.456 839.637 / 1.293 0.004 0.001> #<bodyset-link #X686f240 r_shoulder_lift_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 0.001> #<bodyset-link #X6870620 r_upper_arm_roll_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 -1.57> #<bodyset-link #X68733c8 r_elbow_flex_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 -1.296> #<bodyset-link #X6873500 r_forearm_roll_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 2.074> #<bodyset-link #X6875f78 r_wrist_flex_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 1.566> #<bodyset-link #X68762a8 r_wrist_roll_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 -0.016>) :rthre 0.087266 :thre 10 :stop 300 :additional-weight-list ((#<bodyset-link #X68a5d10 torso_lift_link_lk  3326.791 1302.495 840.013 / 1.544 0.004 0.002> 0.005)) :link-list (#<bodyset-link #X68a9970 r_shoulder_pan_link_lk  3514.723 1297.456 839.637 / 1.293 0.004 0.001> #<bodyset-link #X686f240 r_shoulder_lift_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 0.001> #<bodyset-link #X6870620 r_upper_arm_roll_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 -1.57> #<bodyset-link #X68733c8 r_elbow_flex_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 -1.296> #<bodyset-link #X6873500 r_forearm_roll_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 2.074> #<bodyset-link #X6875f78 r_wrist_flex_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 1.566> #<bodyset-link #X68762a8 r_wrist_roll_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 -0.016>) :move-target #<cascaded-coords #X89da730  3347.209 1835.8 675.741 / 1.521 0.528 -0.016> :dump-command nil :debug-view t :move-target #<cascaded-coords #X89da730  3347.209 1835.8 675.741 / 1.521 0.528 -0.016> :link-list (#<bodyset-link #X68a9970 r_shoulder_pan_link_lk  3514.723 1297.456 839.637 / 1.293 0.004 0.001> #<bodyset-link #X686f240 r_shoulder_lift_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 0.001> #<bodyset-link #X6870620 r_upper_arm_roll_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 -1.57> #<bodyset-link #X68733c8 r_elbow_flex_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 -1.296> #<bodyset-link #X6873500 r_forearm_roll_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 2.074> #<bodyset-link #X6875f78 r_wrist_flex_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 1.566> #<bodyset-link #X68762a8 r_wrist_roll_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 -0.016>) :rthre 0.087266 :thre 10 :stop 300 :additional-weight-list #<bodyset-link #X68a5d10 torso_lift_link_lk  3326.791 1302.495 840.013 / 1.544 0.004 0.002> :link-list (#<bodyset-link #X68a9970 r_shoulder_pan_link_lk  3514.723 1297.456 839.637 / 1.293 0.004 0.001> #<bodyset-link #X686f240 r_shoulder_lift_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 0.001> #<bodyset-link #X6870620 r_upper_arm_roll_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 -1.57> #<bodyset-link #X68733c8 r_elbow_flex_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 -1.296> #<bodyset-link #X6873500 r_forearm_roll_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 2.074> #<bodyset-link #X6875f78 r_wrist_flex_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 1.566> #<bodyset-link #X68762a8 r_wrist_roll_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 -0.016>) :move-target #<cascaded-coords #X79ce7e8  3347.209 1835.8 675.741 / 1.521 0.528 -0.016> :move-target #<cascaded-coords #X7a01240  3347.209 1835.8 675.741 / 1.521 0.528 -0.016> :collision-avoidance-link-pair nil :link-list (#<bodyset-link #X68a9970 r_shoulder_pan_link_lk  3514.723 1297.456 839.637 / 1.293 0.004 0.001> #<bodyset-link #X686f240 r_shoulder_lift_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 0.001> #<bodyset-link #X6870620 r_upper_arm_roll_link_lk  3542.144 1393.622 839.2 / 1.293 0.32 -1.57> #<bodyset-link #X68733c8 r_elbow_flex_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 -1.296> #<bodyset-link #X6873500 r_forearm_roll_link_lk  3646.145 1758.787 713.35 / -2.892 -0.166 2.074> #<bodyset-link #X6875f78 r_wrist_flex_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 1.566> #<bodyset-link #X68762a8 r_wrist_roll_link_lk  3339.393 1680.498 766.405 / 1.521 0.528 -0.016>))

…ll links and list to fix jsk-ros-pkg/jsk_roseus#138 and fix bug in move-target or target-coords

snozawa · 2014-07-16T10:42:53Z

I fixed segmentation fault from :additional-weight-check[1].
After [1], all links in print-args will be replaced by loadable format like (send r :link link-name).

[1] euslisp/jskeus#104

@aginika
Could you update jskeus and create ik fail log again.

The diff results were as below.

dif-pos and dif-rot from ik fail logs seem not to be exactly same value as original ik dif-pos and dif-rot
because floating numbers are dumped as string into ik fail logs.
I'll close this PR because segmentation fault was fixed.
If you have any problem, please reopen this issue or create another issue.

aginika · 2014-07-17T20:48:32Z

@snozawa
Thanks a lot!!! The failur file was perfectly created and there seems not to be any problems.

I run the code with roseus and the ik-check functions seems to work!!

;; ik success log at Fri Jul 18 14:44:15 2014 on (9.00 inagaki Fri Jul 18 14:35:07 JST 2014 e6031bb f332154)
;;
;; link-list (#<bodyset-link #Xc36b4f8 r_shoulder_pan_link  3514.721 1297.459 839.637 / 1.273 0.004 0.001> #<bodyset-link #Xc3d5bd8 r_shoulder_lift_link  3544.072 1393.054 839.198 / 1.273 0.576 0.001> #<bodyset-link #Xc435ec0 r_upper_arm_roll_link  3544.072 1393.054 839.198 / 1.273 0.576 -1.386> #<bodyset-link #Xc2e4670 r_elbow_flex_link  3642.375 1713.859 621.434 / -3.04 -0.429 -1.136> #<bodyset-link #Xc331788 r_forearm_roll_link  3642.375 1713.859 621.434 / -3.04 -0.429 2.022> #<bodyset-link #Xc1b8a28 r_wrist_flex_link  3351.999 1684.125 754.995 / 1.571 0.524 1.905> #<bodyset-link #Xc1fb1a8 r_wrist_roll_link  3351.999 1684.125 754.995 / 1.571 0.524 -1.829e-06>)
;; move-target #<cascaded-coords #Xc5e7820 :rarm-end-coords  3351.999 1840.01 664.995 / 1.571 0.524 -1.829e-06>
;; rotatoin-axis t, translation-axis t
;; thre 10, rthre 0.087266, stop 300
(setq c0 '(#s(coordinates plist nil rot #2f((3.174644e-06 -1.0 3.960159e-09) (0.866025 2.751300e-06 0.500001) (-0.500001 -1.583895e-06 0.866025)) pos #f(3352.0 1805.37 684.995))))
(setq av0 #f(50.0 110.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -15.5326 32.7327 -79.4761 -121.542 -179.057 -106.754 250.856 -1.12702 58.7871))
(defun pr2-sensor-robot-2014-07-18-14-44-15-setup () (let ((r (instance pr2-sensor-robot :init))) (setq *robot* r) (progn (send r :newcoords (make-coords :4x4 #2f((0.026793 -0.999639 0.002106 3326.36) (0.999633 0.026801 0.003945 1349.16) (-0.004 0.002 0.99999 -0.854) (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 110.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -15.5326 32.7327 -79.4761 -121.542 -179.057 -106.754 250.856 -1.12702 58.7871)) (objects (list r))) (objects (list *robot*))))
(defun pr2-sensor-robot-2014-07-18-14-44-15-check () (let ((r *robot*)) (send* r :inverse-kinematics (list (list (make-coords :pos #f(3352.0 1805.37 684.995) :rot #2f((3.174644e-06 -1.0 3.960159e-09) (0.866025 2.751300e-06 0.500001) (-0.500001 -1.583895e-06 0.866025)))) :dump-command nil :debug-view t :move-target (let* ((p (send r :link "r_wrist_roll_link")) (mt (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-coords :4x4 #2f((1.0 0.0 0.0 180.0) (0.0 1.0 0.0 4.547474e-13) (0.0 0.0 1.0 2.273737e-13) (0.0 0.0 0.0 1.0))))))) (send p :assoc mt) mt) :link-list (list (send r :link "r_shoulder_pan_link") (send r :link "r_shoulder_lift_link") (send r :link "r_upper_arm_roll_link") (send r :link "r_elbow_flex_link") (send r :link "r_forearm_roll_link") (send r :link "r_wrist_flex_link") (send r :link "r_wrist_roll_link")) :rthre 0.087266 :thre 10 :stop 300 :additional-weight-list (list (list (send r :link "torso_lift_link") 0.005)) :link-list (list (send r :link "r_shoulder_pan_link") (send r :link "r_shoulder_lift_link") (send r :link "r_upper_arm_roll_link") (send r :link "r_elbow_flex_link") (send r :link "r_forearm_roll_link") (send r :link "r_wrist_flex_link") (send r :link "r_wrist_roll_link")) :move-target (let* ((p (send r :link "r_wrist_roll_link")) (mt (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-coords :4x4 #2f((1.0 0.0 0.0 180.0) (0.0 1.0 0.0 4.547474e-13) (0.0 0.0 1.0 2.273737e-13) (0.0 0.0 0.0 1.0))))))) (send p :assoc mt) mt) :move-target (let* ((p (send r :link "r_wrist_roll_link")) (mt (make-cascoords :coords (send (send p :copy-worldcoords) :transform (make-coords :4x4 #2f((1.0 0.0 0.0 180.0) (0.0 1.0 0.0 4.547474e-13) (0.0 0.0 1.0 2.273737e-13) (0.0 0.0 0.0 1.0))))))) (send p :assoc mt) mt) :collision-avoidance-link-pair nil :link-list (list (send r :link "r_shoulder_pan_link") (send r :link "r_shoulder_lift_link") (send r :link "r_upper_arm_roll_link") (send r :link "r_elbow_flex_link") (send r :link "r_forearm_roll_link") (send r :link "r_wrist_flex_link") (send r :link "r_wrist_roll_link"))))))
(defun ik-setup () (pr2-sensor-robot-2014-07-18-14-44-15-setup))
(defun ik-check () (pr2-sensor-robot-2014-07-18-14-44-15-check))
(setq ik-failed '((:dif-pos . (#f(-30.7121 -6.19955 -7.29337))) (:dif-rot . (#f(-0.009168 -0.004614 0.043459))

aginika closed this as completed Jul 13, 2014

k-okada reopened this Jul 13, 2014

snozawa added a commit to snozawa/jskeus that referenced this issue Jul 16, 2014

(irtmodel.l, test-irt-motion.l) : Update dump ik fail log to escape a…

d740c10

…ll links and list to fix jsk-ros-pkg/jsk_roseus#138 and fix bug in move-target or target-coords

snozawa mentioned this issue Jul 16, 2014

Update dump ik fail log euslisp/jskeus#104

Merged

snozawa closed this as completed in euslisp/jskeus#104 Jul 16, 2014

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

/tmp/irtmodel-ik failuer file will become segmentation fault when I execute with roseus #138

/tmp/irtmodel-ik failuer file will become segmentation fault when I execute with roseus #138

aginika commented Jul 12, 2014

k-okada commented Jul 13, 2014

aginika commented Jul 13, 2014

k-okada commented Jul 13, 2014

aginika commented Jul 13, 2014

k-okada commented Jul 13, 2014

aginika commented Jul 13, 2014

aginika commented Jul 13, 2014

snozawa commented Jul 16, 2014

aginika commented Jul 17, 2014

/tmp/irtmodel-ik failuer file will become segmentation fault when I execute with roseus #138

/tmp/irtmodel-ik failuer file will become segmentation fault when I execute with roseus #138

Comments

aginika commented Jul 12, 2014

k-okada commented Jul 13, 2014

aginika commented Jul 13, 2014

k-okada commented Jul 13, 2014

aginika commented Jul 13, 2014

k-okada commented Jul 13, 2014

aginika commented Jul 13, 2014

aginika commented Jul 13, 2014

snozawa commented Jul 16, 2014

aginika commented Jul 17, 2014