From 69b2aa936076caf297f68f9beb739db3e9913388 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 30 May 2016 16:22:59 +0900 Subject: [PATCH] fix failing remove gripper link from link-list, (member 'string' list) requries (member 'string' list #'equal) --- .../euslisp/jsk_2016_01_baxter_apc/baxter.l | 2 +- jsk_2016_01_baxter_apc/test/test-ik.l | 17 ++++++++++++++--- 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter.l b/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter.l index fec6de94d..1c75b1de1 100644 --- a/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter.l +++ b/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter.l @@ -43,7 +43,7 @@ &key link-list &allow-other-keys) ;; if the last lik of link-list is a gripper, remove them - (when (member (send (car (last link-list)) :name) (list "right_gripper_vacuum_pad_base" "left_gripper_vacuum_pad_base")) + (when (member (send (car (last link-list)) :name) (list "right_gripper_vacuum_pad_base" "left_gripper_vacuum_pad_base") :test #'equal) (setq link-list (butlast link-list))) (send-super* :inverse-kinematics target-coords :link-list link-list args) diff --git a/jsk_2016_01_baxter_apc/test/test-ik.l b/jsk_2016_01_baxter_apc/test/test-ik.l index 0cd512e55..94c51fccc 100755 --- a/jsk_2016_01_baxter_apc/test/test-ik.l +++ b/jsk_2016_01_baxter_apc/test/test-ik.l @@ -26,14 +26,25 @@ ))) (deftest issue-1474 () ;; https://github.com/start-jsk/jsk_apc/issues/1470#issuecomment-220518802 - (let (av1 av2) + (let (av1 av2 av3) (setup) (setq av1 (send *baxter* :angle-vector)) - (send *baxter* :rarm :move-end-pos #f(0 0 -50) :local) + ;;(send *baxter* :rarm :move-end-pos #f(0 0 -50) :local) + (send-message *baxter* baxter-robot :rarm :move-end-pos #f(0 0 -50) :local) (setq av2 (send *baxter* :angle-vector)) ;; #f(0.0 97.4707 -2.39502 -94.5483 134.67 91.4062 8.70117 0.0 -8.22871 40.7953 92.2281 64.6958 -80.3615 -21.3969 -46.1696 90.0) (check-result av1 av2) (assert (and av1 av2)) - (assert (< (norm (v- av1 av2)) 10)) + (assert (< (norm (v- av1 av2)) 90)) ;; + ;; add to check if this can be solve normal-ik + (send *baxter* :angle-vector av1) + (setq av3 (send-message *baxter* robot-model + :inverse-kinematics (send (send *baxter* :rarm :end-coords :copy-worldcoords) :translate #f(0 0 -50)) + :move-target (send *baxter* :rarm :end-coords) + :link-list (send *baxter* :link-list (send *baxter* :right_gripper_vacuum_pad_joint :parent-link) (send *baxter* :rarm :root-link)) + :debug-view t)) + (check-result av2 av3) + (assert (and av2 av3)) + (assert (< (norm (v- av2 av3)) 5)) ;; )) ;; https://github.com/euslisp/jskeus/pull/380