Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix invalid indexing in collision link pair calculation #461

Merged
merged 7 commits into from Jul 17, 2018

Conversation

Projects
None yet
2 participants
@k-okada
Copy link
Member

commented Oct 18, 2017

Closes #459

Override #460

Add test code to check invalid indexing of collision link pair calculation
(#459)

@snozawa

This comment has been minimized.

Copy link
Contributor

commented Oct 20, 2017

LGTM.
(but travis fails??)

@k-okada

This comment has been minimized.

Copy link
Member Author

commented Oct 20, 2017

test-samplerobot-walk-pattern-ntimes failing...

https://s3.amazonaws.com/archive.travis-ci.org/jobs/289750665/log.txt?X-Amz-Expires=29&X-Amz-Date=20171020T094603Z&X-Amz-Algorithm=AWS4-HMAC-SHA256&X-Amz-Credential=AKIAJRYRXRSVGNKPKO5A/20171020/us-east-1/s3/aws4_request&X-Amz-SignedHeaders=host&X-Amz-Signature=021c2cda657c85ba9da13cf5074a5151ffc5bbb932a0cda0595b4e8c80773e40

�[0mTEST-NAME: test-samplerobot-walk-pattern-ntimes
  now testing...
�[32mstart testing [test-samplerobot-walk-pattern-ntimes]
�[0m�[31m[ERROR] test (eps= (elt cog0 1) (- (elt cog2 1) 100) 0.5) failed ... ( ^[0mmstart testing [test-samplerobot-walk-pattern-ntimes]
angle(-175) violate min-angle(-135)
 t
0.523599 1.0472)
ged-robot-with-interlocking-joints-2017-10-19-00-48-54-73-40-99-success.l)
st (list (send r :link :rarm-link0) (send r :link :rarm-link1) (sen).�[0m

https://s3.amazonaws.com/archive.travis-ci.org/jobs/289749131/log.txt?X-Amz-Expires=30&X-Amz-Date=20171020T095234Z&X-Amz-Algorithm=AWS4-HMAC-SHA256&X-Amz-Credential=AKIAJRYRXRSVGNKPKO5A/20171020/us-east-1/s3/aws4_request&X-Amz-SignedHeaders=host&X-Amz-Signature=f1354527de442c4ed3665d12f5e9982edf485dff1591d5920573346e1435c1f8

�[0mCall Stack (max depth: 20):
  0: at (send *sample-robot* :collision-avoidance-link-pair-from-link-list (send *sample-robot* :link-list (send (send *sample-robot* :rarm :end-coords) :parent) (send (send *sample-robot* :rarm :wrist-y) :child-link)))
  1: at (funcall func)
  2: at (funcall func)
  3: at (let ((func (symbol-function func-sym)) tm) (send *unit-test* :init-result func-sym) (format t "TEST-NAME: ~A~%" func-sym) (format t "  now testing...~%") (send *unit-test* :increment-test func-sym) (setq tm (instance mtimer :init)) (send tm :start) (funcall func) (send *unit-test* :set-time-to-current-result (send tm :stop)))
  4: at (run-test u)
  5: at (while #:dolist128 (setq u (pop #:dolist128)) (run-test u))
  6: at (let ((u nil) (#:dolist128 (send *unit-test* :functions))) nil (while #:dolist128 (setq u (pop #:dolist128)) (run-test u)) nil)
  7: at (dolist (u (send *unit-test* :functions)) (run-test u))
  8: at (run-all-tests)
�[1;31mirteusgl unittest-error: sequence index out of range in (send *sample-robot* :collision-avoidance-link-pair-from-link-list (send *sample-robot* :link-list (send (send *sample-robot* :rarm :end-coords) :parent) (send (send *sample-robot* :rarm :wrist-y) :child-link))), exitting...�[0m
�[31m[ERROR] test (send *sample-robot* :collision-avoidance-link-pair-from-link-list (send *sample-robot* :link-list (send (send *sample-robot* :rarm :end-coords) :parent) (send (send *sample-robot* :rarm :wrist-y) :child-link))) failed ... (sequence index out of range 72).�[0m

@k-okada k-okada force-pushed the k-okada:fix_bug_of_collink_index branch from 37ef085 to 85e78d4 Oct 20, 2017

@snozawa

This comment has been minimized.

Copy link
Contributor

commented Oct 23, 2017

According to test results, the error messsages are as follows:
https://travis-ci.org/euslisp/jskeus/builds/290436618?utm_source=github_status&utm_medium=notification

TEST-NAME: test-orientation-of-joint-class
  now testing...
�[32mstart testing [test-orientation-of-joint-class]
�[0m;;1 face-to-face alignment(s) found.
Call Stack (max depth: 20):
  0: at (send *robot* :inverse-kinematics-loop dp dr :link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :union-link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :move-target (send *robot* :end-coords) :avoid-nspace-gain 0.0 :avoid-weight-gain 0.0 :thre 0 :rthre 0 :p-limit 1000 :r-limit 1000)
  1: at (send *robot* :inverse-kinematics-loop dp dr :link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :union-link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :move-target (send *robot* :end-coords) :avoid-nspace-gain 0.0 :avoid-weight-gain 0.0 :thre 0 :rthre 0 :p-limit 1000 :r-limit 1000)
  2: at (send *robot* :inverse-kinematics-loop dp dr :link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :union-link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :move-target (send *robot* :end-coords) :avoid-nspace-gain 0.0 :avoid-weight-gain 0.0 :thre 0 :rthre 0 :p-limit 1000 :r-limit 1000)
  3: at (let ((dp (send #:dotimes476 :difference-position #:dotimes477 :translation-axis #:dotimes479)) (dr (send #:dotimes476 :difference-rotation #:dotimes477 :rotation-axis #:dotimes478))) (send *robot* :inverse-kinematics-loop dp dr :link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :union-link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :move-target (send *robot* :end-coords) :avoid-nspace-gain 0.0 :avoid-weight-gain 0.0 :thre 0 :rthre 0 :p-limit 1000 :r-limit 1000))
  4: at (let ((#:dotimes476 (send *robot* :end-coords)) (#:dotimes477 ec) (#:dotimes479 t) (#:dotimes478 t)) (let ((dp (send #:dotimes476 :difference-position #:dotimes477 :translation-axis #:dotimes479)) (dr (send #:dotimes476 :difference-rotation #:dotimes477 :rotation-axis #:dotimes478))) (send *robot* :inverse-kinematics-loop dp dr :link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :union-link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :move-target (send *robot* :end-coords) :avoid-nspace-gain 0.0 :avoid-weight-gain 0.0 :thre 0 :rthre 0 :p-limit 1000 :r-limit 1000)))
  5: at (with-difference-position-and-rotation (dp dr (send *robot* :end-coords) ec) (send *robot* :inverse-kinematics-loop dp dr :link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :union-link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :move-target (send *robot* :end-coords) :avoid-nspace-gain 0.0 :avoid-weight-gain 0.0 :thre 0 :rthre 0 :p-limit 1000 :r-limit 1000))
  6: at (let ((prev-ec ec) (prev-ja ja) (prev-cc cc) (dd (* range (sin (/ i div))))) (setq ja (let ((tmp-ja (instantiate float-vector (send *robot* :joint :joint-dof)))) (dotimes (i (send *robot* :joint :joint-dof)) (setf (elt tmp-ja i) dd)) tmp-ja)) (send *robot* :joint :joint-angle ja) (setq ec (send *robot* :end-coords :copy-worldcoords)) (setq cc (send *robot* :joint :child-link :copy-worldcoords)) (send *robot* :joint :joint-angle prev-ja) (with-difference-position-and-rotation (dp dr (send *robot* :end-coords) ec) (send *robot* :inverse-kinematics-loop dp dr :link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :union-link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :move-target (send *robot* :end-coords) :avoid-nspace-gain 0.0 :avoid-weight-gain 0.0 :thre 0 :rthre 0 :p-limit 1000 :r-limit 1000)) (setq calc-ec (send *robot* :end-coords :copy-worldcoords)) (if (> (norm (send ec :difference-position calc-ec)) max-dif-pos) (setq max-dif-pos (norm (send ec :difference-position calc-ec)))) (if (> (norm (send ec :difference-rotation calc-ec)) max-dif-rot) (setq max-dif-rot (norm (send ec :difference-rotation calc-ec)))) (unless (or (null x:*display*) (= x:*display* 0)) (send *irtviewer* :draw-objects :flush nil) (send ec :draw-on :flush nil :size 400) (send calc-ec :draw-on :flush nil :color #f(1.0 0.0 0.0)) (send *irtviewer* :viewer :flush)))
  7: at (while (< i #:dotimes474) (let ((prev-ec ec) (prev-ja ja) (prev-cc cc) (dd (* range (sin (/ i div))))) (setq ja (let ((tmp-ja (instantiate float-vector (send *robot* :joint :joint-dof)))) (dotimes (i (send *robot* :joint :joint-dof)) (setf (elt tmp-ja i) dd)) tmp-ja)) (send *robot* :joint :joint-angle ja) (setq ec (send *robot* :end-coords :copy-worldcoords)) (setq cc (send *robot* :joint :child-link :copy-worldcoords)) (send *robot* :joint :joint-angle prev-ja) (with-difference-position-and-rotation (dp dr (send *robot* :end-coords) ec) (send *robot* :inverse-kinematics-loop dp dr :link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :union-link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :move-target (send *robot* :end-coords) :avoid-nspace-gain 0.0 :avoid-weight-gain 0.0 :thre 0 :rthre 0 :p-limit 1000 :r-limit 1000)) (setq calc-ec (send *robot* :end-coords :copy-worldcoords)) (if (> (norm (send ec :difference-position calc-ec)) max-dif-pos) (setq max-dif-pos (norm (send ec :difference-position calc-ec)))) (if (> (norm (send ec :difference-rotation calc-ec)) max-dif-rot) (setq max-dif-rot (norm (send ec :difference-rotation calc-ec)))) (unless (or (null x:*display*) (= x:*display* 0)) (send *irtviewer* :draw-objects :flush nil) (send ec :draw-on :flush nil :size 400) (send calc-ec :draw-on :flush nil :color #f(1.0 0.0 0.0)) (send *irtviewer* :viewer :flush))) (setq i (1+ i)))
  8: at (let ((i 0) (#:dotimes474 600)) (declare (integer i #:dotimes474)) (while (< i #:dotimes474) (let ((prev-ec ec) (prev-ja ja) (prev-cc cc) (dd (* range (sin (/ i div))))) (setq ja (let ((tmp-ja (instantiate float-vector (send *robot* :joint :joint-dof)))) (dotimes (i (send *robot* :joint :joint-dof)) (setf (elt tmp-ja i) dd)) tmp-ja)) (send *robot* :joint :joint-angle ja) (setq ec (send *robot* :end-coords :copy-worldcoords)) (setq cc (send *robot* :joint :child-link :copy-worldcoords)) (send *robot* :joint :joint-angle prev-ja) (with-difference-position-and-rotation (dp dr (send *robot* :end-coords) ec) (send *robot* :inverse-kinematics-loop dp dr :link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :union-link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :move-target (send *robot* :end-coords) :avoid-nspace-gain 0.0 :avoid-weight-gain 0.0 :thre 0 :rthre 0 :p-limit 1000 :r-limit 1000)) (setq calc-ec (send *robot* :end-coords :copy-worldcoords)) (if (> (norm (send ec :difference-position calc-ec)) max-dif-pos) (setq max-dif-pos (norm (send ec :difference-position calc-ec)))) (if (> (norm (send ec :difference-rotation calc-ec)) max-dif-rot) (setq max-dif-rot (norm (send ec :difference-rotation calc-ec)))) (unless (or (null x:*display*) (= x:*display* 0)) (send *irtviewer* :draw-objects :flush nil) (send ec :draw-on :flush nil :size 400) (send calc-ec :draw-on :flush nil :color #f(1.0 0.0 0.0)) (send *irtviewer* :viewer :flush))) (setq i (1+ i))) nil)
  9: at (dotimes (i 600) (let ((prev-ec ec) (prev-ja ja) (prev-cc cc) (dd (* range (sin (/ i div))))) (setq ja (let ((tmp-ja (instantiate float-vector (send *robot* :joint :joint-dof)))) (dotimes (i (send *robot* :joint :joint-dof)) (setf (elt tmp-ja i) dd)) tmp-ja)) (send *robot* :joint :joint-angle ja) (setq ec (send *robot* :end-coords :copy-worldcoords)) (setq cc (send *robot* :joint :child-link :copy-worldcoords)) (send *robot* :joint :joint-angle prev-ja) (with-difference-position-and-rotation (dp dr (send *robot* :end-coords) ec) (send *robot* :inverse-kinematics-loop dp dr :link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :union-link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :move-target (send *robot* :end-coords) :avoid-nspace-gain 0.0 :avoid-weight-gain 0.0 :thre 0 :rthre 0 :p-limit 1000 :r-limit 1000)) (setq calc-ec (send *robot* :end-coords :copy-worldcoords)) (if (> (norm (send ec :difference-position calc-ec)) max-dif-pos) (setq max-dif-pos (norm (send ec :difference-position calc-ec)))) (if (> (norm (send ec :difference-rotation calc-ec)) max-dif-rot) (setq max-dif-rot (norm (send ec :difference-rotation calc-ec)))) (unless (or (null x:*display*) (= x:*display* 0)) (send *irtviewer* :draw-objects :flush nil) (send ec :draw-on :flush nil :size 400) (send calc-ec :draw-on :flush nil :color #f(1.0 0.0 0.0)) (send *irtviewer* :viewer :flush))))
  10: at (let ((div 100.0) (max-dif-pos 0.0) (max-dif-rot 0.0) (range 90) (ja (send *robot* :joint :joint-angle)) (ec (send *robot* :end-coords :copy-worldcoords)) (cc (send *robot* :joint :child-link :copy-worldcoords))) (dotimes (i 600) (let ((prev-ec ec) (prev-ja ja) (prev-cc cc) (dd (* range (sin (/ i div))))) (setq ja (let ((tmp-ja (instantiate float-vector (send *robot* :joint :joint-dof)))) (dotimes (i (send *robot* :joint :joint-dof)) (setf (elt tmp-ja i) dd)) tmp-ja)) (send *robot* :joint :joint-angle ja) (setq ec (send *robot* :end-coords :copy-worldcoords)) (setq cc (send *robot* :joint :child-link :copy-worldcoords)) (send *robot* :joint :joint-angle prev-ja) (with-difference-position-and-rotation (dp dr (send *robot* :end-coords) ec) (send *robot* :inverse-kinematics-loop dp dr :link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :union-link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :move-target (send *robot* :end-coords) :avoid-nspace-gain 0.0 :avoid-weight-gain 0.0 :thre 0 :rthre 0 :p-limit 1000 :r-limit 1000)) (setq calc-ec (send *robot* :end-coords :copy-worldcoords)) (if (> (norm (send ec :difference-position calc-ec)) max-dif-pos) (setq max-dif-pos (norm (send ec :difference-position calc-ec)))) (if (> (norm (send ec :difference-rotation calc-ec)) max-dif-rot) (setq max-dif-rot (norm (send ec :difference-rotation calc-ec)))) (unless (or (null x:*display*) (= x:*display* 0)) (send *irtviewer* :draw-objects :flush nil) (send ec :draw-on :flush nil :size 400) (send calc-ec :draw-on :flush nil :color #f(1.0 0.0 0.0)) (send *irtviewer* :viewer :flush)))) (list max-dif-pos max-dif-rot))
  11: at (test-orientation-of-joint-class-common sphere-joint)
  12: at (let ((ret (test-orientation-of-joint-class-common sphere-joint))) (and (> 0.01 (elt ret 0)) (> (deg2rad 0.01) (elt ret 1))))
  13: at (let* (failure (ret (let ((ret (test-orientation-of-joint-class-common sphere-joint))) (and (> 0.01 (elt ret 0)) (> (deg2rad 0.01) (elt ret 1)))))) (if (not ret) (send *unit-test* :increment-failure '(let ((ret (test-orientation-of-joint-class-common sphere-joint))) (and (> 0.01 (elt ret 0)) (> (deg2rad 0.01) (elt ret 1)))) (format nil "") (escape-xml-string (subseq (send *error-output* :buffer) 0 (or (position 0 (send *error-output* :buffer)) (length (send *error-output* :buffer))))))))
  14: at (funcall func)
  15: at (funcall func)
  16: at (let ((func (symbol-function func-sym)) tm) (send *unit-test* :init-result func-sym) (format t "TEST-NAME: ~A~%" func-sym) (format t "  now testing...~%") (send *unit-test* :increment-test func-sym) (setq tm (instance mtimer :init)) (send tm :start) (funcall func) (send *unit-test* :set-time-to-current-result (send tm :stop)))
  17: at (run-test u)
  18: at (while #:dolist128 (setq u (pop #:dolist128)) (run-test u))
  19: at (let ((u nil) (#:dolist128 (send *unit-test* :functions))) nil (while #:dolist128 (setq u (pop #:dolist128)) (run-test u)) nil)
  And more...
�[1;31mirteusgl unittest-error: illegal start/end index in (send *robot* :inverse-kinematics-loop dp dr :link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :union-link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :move-target (send *robot* :end-coords) :avoid-nspace-gain 0.0 :avoid-weight-gain 0.0 :thre 0 :rthre 0 :p-limit 1000 :r-limit 1000), exitting...�[0m
�[31m[ERROR] test (send *robot* :inverse-kinematics-loop dp dr :link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :union-link-list (send *robot* :link-list (send *robot* :end-coords :parent)) :move-target (send *robot* :end-coords) :avoid-nspace-gain 0.0 :avoid-weight-gain 0.0 :thre 0 :rthre 0 :p-limit 1000 :r-limit 1000) failed ... (illegal start/end index 50).�[0m
ALL RESULTS:
  TEST-NUM: 5
    PASSED:   4
�[31m    FAILURE:  1
@snozawa

This comment has been minimized.

Copy link
Contributor

commented Oct 23, 2017

According to this message, I found the following commit does not work in the case that (length link-list) is 1:
8317a52

test-orientation-joint-hogehoge tests one dof joint and link-list length is 1.
In this case, subseq start index should be 0:

(setq link-list (list 0)) ;; 1 length
(subseq link-list 0) ;; OK
(subseq link-list 1) ;; NG
@snozawa

This comment has been minimized.

Copy link
Contributor

commented Oct 23, 2017

How about the following solutions?

  1. Revert 8317a52

or

  1. In addition to 8317a52, add the following minimum length checking:
diff --git a/irteus/irtmodel.l b/irteus/irtmodel.l
index 04efb10..8b8a416 100644
--- a/irteus/irtmodel.l
+++ b/irteus/irtmodel.l
@@ -1276,7 +1276,7 @@
         (setq valid-link-list
               (subseq  ;; returns link-list when it's length is 1
                link-list
-               (do ((i 1 (1+ i)))
+               (do ((i (min 1 (1- (length link-list))) (1+ i)))
                    ((not (and (< i (1- (length link-list)))
                               (eps= (distance (send (elt link-list 0) :worldpos
                                               (send (elt link-list 1) :worldpos

@k-okada k-okada force-pushed the k-okada:fix_bug_of_collink_index branch from 272ad89 to 20c2123 Nov 3, 2017

@k-okada k-okada force-pushed the k-okada:fix_bug_of_collink_index branch from 319085d to 15d2b70 Nov 3, 2017

@k-okada

This comment has been minimized.

Copy link
Member Author

commented Nov 3, 2017

@snozawa, please check last commit (5a4ba71), I increased error tolerance. How do you think?

@k-okada k-okada added the PR/MergeOK label Jul 11, 2018

@k-okada k-okada force-pushed the k-okada:fix_bug_of_collink_index branch from 23de77a to 8317a52 Jul 13, 2018

@k-okada k-okada merged commit 6cad16f into euslisp:master Jul 17, 2018

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details

@k-okada k-okada deleted the k-okada:fix_bug_of_collink_index branch Jul 17, 2018

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.