From a06e1f9229ea548c2e137a0f278b55a026cb69e1 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Fri, 17 Jul 2015 01:16:22 +0900 Subject: [PATCH 1/3] [irteus/irtmodel.l] Add check collision argument to :inverse-kinematics and nil by default according to https://github.com/euslisp/jskeus/issues/95 --- irteus/irtmodel.l | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/irteus/irtmodel.l b/irteus/irtmodel.l index 395b5fd55..518264956 100644 --- a/irteus/irtmodel.l +++ b/irteus/irtmodel.l @@ -2104,6 +2104,7 @@ (union-link-list) (centroid-thre 1.0) (target-centroid-pos) (centroid-offset-func) (cog-translation-axis :z) (dump-command t) (periodic-time 0.5) ;; [s] + (check-collision) &allow-other-keys) "Move move-target to target-coords." ;; target-coords, move-target, rotation-axis, translation-axis, link-list @@ -2119,7 +2120,7 @@ ;; (success t) (old-analysis-level (send-all union-link-list :analysis-level)) - command-directory command-filename command-id ik-args) + command-directory command-filename command-id ik-args collision-status) (send-all union-link-list :analysis-level :coords) ;; argument check (when (or (null link-list) (null move-target)) @@ -2230,8 +2231,9 @@ (unix::rename (format nil "~A/~A.l" command-directory command-id ) (setq command-filename (format nil "~A/~A-~A.l" command-directory command-id (if (or success (not revert-if-fail)) "success" "failure"))))) ;; reset weight (send self :reset-joint-angle-limit-weight-old union-link-list) + (if check-collision (setq collision-status (send self :self-collision-check))) ;; check solved or not - (if (or success (not revert-if-fail)) + (if (and (or success (not revert-if-fail)) (not collision-status)) (send self :angle-vector) (progn (when warnp @@ -2245,6 +2247,7 @@ (warn ";; coords : ~a~%" (send (let ((p self)) (while (send p :parent) (setq p (send p :parent))) p) :worldcoords)) (warn ";; angles : ~a~%" av0) + (if check-collision (warn ";; collision : ~a~%" collision-status)) (warn ";; args : ~a~%" (append (list target-coords) args)) (when dump-command (let (i print-args command-init command-setup command-args) From 893cacf126f02c8a312074cfa72a6df79b9999bd Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Fri, 17 Jul 2015 01:17:03 +0900 Subject: [PATCH 2/3] [irteus/irtmodel.l] Make pqpmodel in :init-ending --- irteus/irtmodel.l | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/irteus/irtmodel.l b/irteus/irtmodel.l index 518264956..45cf63d7a 100644 --- a/irteus/irtmodel.l +++ b/irteus/irtmodel.l @@ -747,7 +747,10 @@ (send (send j :child-link) :add-parent-link (send j :parent-link)) (send (send j :parent-link) :add-child-links (send j :child-link)) ) - (send self :update-descendants)) + (send self :update-descendants) + ;; Make default collision shape by pqp + (dolist (l (send self :links)) (send l :make-pqpmodel)) + ) (:links (&rest args) "Returns links, or args is passed to links" (user::forward-message-to-all links args)) (:joint-list (&rest args) "Returns joint list, or args is passed to joints" (user::forward-message-to-all joint-list args)) (:link (name) "Return a link with given name." (find name links :test #'equal :key #'(lambda (x) (send x :name)))) From 0c6febd3cdb5f4f32122156467f3886a652ef5e3 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Fri, 17 Jul 2015 01:17:13 +0900 Subject: [PATCH 3/3] [irteus/test/test-irt-motion.l] Add test program for check-collision --- irteus/test/test-irt-motion.l | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/irteus/test/test-irt-motion.l b/irteus/test/test-irt-motion.l index d732408af..092459ba3 100644 --- a/irteus/test/test-irt-motion.l +++ b/irteus/test/test-irt-motion.l @@ -674,6 +674,29 @@ (every #'identity check-results) )))) +;; test for collision check +(defun test-self-collision-check-IK + () + (defmethod sample-robot + (:collision-check-pairs + (&key ((:links ls) (list (car (send self :links)) + (elt (send self :rarm :links) 2) + (elt (send self :rarm :links) 3)))) + (send-super :collision-check-pairs :links ls))) + (labels ((test-ik + (check-collision) + (send *sample-robot* :reset-pose) + (send *sample-robot* :fix-leg-to-coords (make-coords)) + (send *sample-robot* :rarm :move-end-pos #f(50 300 0) :world :rotation-axis nil :check-collision check-collision :warnp nil))) + (prog1 + (and (test-ik nil) (not (test-ik t))) + (defmethod sample-robot + (:collision-check-pairs + (&key ((:links ls) (cons (car links) (all-child-links (car links))))) + (send-super :collision-check-pairs :links ls))) + ) + )) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ;; unit tests ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; @@ -736,5 +759,9 @@ (assert (test-all-for-sample-robot :static-p t :fix-pos #f(100 200 300)))) +(deftest test-self-collision-check-IK-test + (assert + (test-self-collision-check-IK))) + (run-all-tests) (exit 0)