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.

Sign up for GitHub

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

Jump to bottom

Fix invalid indexing in collision link pair calculation #461

Merged
merged 7 commits into from
Jul 17, 2018
Merged

Fix invalid indexing in collision link pair calculation #461

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
85e78d4
Add test code to check invalid indexing of collision link pair calcul…
snozawa Oct 17, 2017
9be89b4
Add checking of index in collision link pair calculation (#459)
k-okada Oct 18, 2017
8317a52
remove (= (length link-list) 1) code, that should be included in subs…
k-okada Oct 20, 2017
20c2123
returns (i - 1) ??
k-okada Nov 2, 2017
15d2b70
add more information (test-samplerobot-walk-pattern-ntimes)
k-okada Nov 3, 2017
5a4ba71
increase assert error for [test-samplerobot-walk-pattern-ntimes]
k-okada Nov 3, 2017
23de77a
Merge branch 'master' into fix_bug_of_collink_index
k-okada Jul 11, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 9 additions & 8 deletions irteus/irtmodel.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1270,15 +1270,16 @@
(warn "collision-links : ~A~%" (send-all collision-links :name))
(warn "parnet-links : ~A~%" (send-all parent-links :name)))
(dolist (link-list link-lists)
;; https://github.com/euslisp/jskeus/issues/459
;; originally introduced in https://github.com/euslisp/jskeus/commit/b0e3b482f8469241b2bfd9e9c607c19542b4d234
;; heuristics to skip adjacent links with same axis
(setq valid-link-list
(if (= (length link-list) 1)
link-list
(subseq
link-list
(let ((i 1))
(while (eps= (distance (send (elt link-list 0) :worldpos)
(send (elt link-list i) :worldpos)) 0.0)
(incf i)) i))))
(subseq ;; returns link-list when it's length is 1
link-list
(do ((i 1 (1+ i)))
((not (and (< i (1- (length link-list)))
(eps= (distance (send (elt link-list 0) :worldpos)
(send (elt link-list i) :worldpos)) 0.0))) (1- i)))))
(setq index-links
(mapcar #'(lambda (l) (position l valid-link-list)) collision-links))
(when (and debug (not (memq :no-message debug)))
Expand Down
17 changes: 15 additions & 2 deletions irteus/test/test-irt-motion.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1011,8 +1011,8 @@
(make-coords :coords (send (send *sample-robot* :lleg :end-coords :copy-worldcoords) :translate (float-vector 0 100 0)) :name :lleg)
(make-coords :coords (send (send *sample-robot* :rleg :end-coords :copy-worldcoords) :translate (float-vector 0 100 0)) :name :rleg))))
(setq cog2 (cadr (memq :cog (car av-list))))
(assert (eps= (elt cog0 1) (elt cog1 1) 0.5))
(assert (eps= (elt cog0 1) (- (elt cog2 1) 100) 0.5))
(assert (eps= (elt cog0 1) (elt cog1 1) 1.5) (format nil "cog: ~A ~A" cog0 cog1))
(assert (eps= (elt cog0 1) (- (elt cog2 1) 100) 1.5) (format nil "cog: ~A ~A" cog0 cog2))
))

(deftest test-samplerobot-static-balance-point
Expand Down Expand Up @@ -1121,6 +1121,19 @@
(send (send root-move-target :parent) :dissoc root-move-target)
(assert ret "Test multiple link-list with root-link and IK"))))

;; test to check https://github.com/euslisp/jskeus/issues/459 issue
(deftest test-sample-robot-collision-avoidance-link-pair-from-link-list
(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)))
(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 :elbow-p) :child-link)))
(assert t)
)

;; Check :cog-translation-axis dimension bug
(deftest test-cog-translation-axis-dim
(unwind-protect
Expand Down