diff --git a/jsk_2016_01_baxter_apc/CMakeLists.txt b/jsk_2016_01_baxter_apc/CMakeLists.txt index 370f9cd34..96c20a6c7 100644 --- a/jsk_2016_01_baxter_apc/CMakeLists.txt +++ b/jsk_2016_01_baxter_apc/CMakeLists.txt @@ -245,6 +245,7 @@ if(CATKIN_ENABLE_TESTING) endif() add_rostest(test/test_move_arm_to_bin.test) add_rostest(test/test_ik_in_bin.test) + add_rostest(test/test_ik.test) find_package(roslaunch REQUIRED) roslaunch_add_file_check(launch/include/vacuum_gripper.launch) find_package(jsk_tools REQUIRED) 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 15560eac0..fec6de94d 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 @@ -40,12 +40,13 @@ (:inverse-kinematics (target-coords &rest args - &key (additional-weight-list - (mapcar #'(lambda (jn) (list (send self jn :child-link) 0)) - '(:right_gripper_vacuum_pad_joint))) + &key link-list &allow-other-keys) - (send-super* :inverse-kinematics target-coords - :additional-weight-list additional-weight-list args) + ;; 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")) + (setq link-list (butlast link-list))) + (send-super* :inverse-kinematics target-coords :link-list link-list + args) ) (:rotate-wrist (arm angle) diff --git a/jsk_2016_01_baxter_apc/test/test-ik.l b/jsk_2016_01_baxter_apc/test/test-ik.l new file mode 100755 index 000000000..0cd512e55 --- /dev/null +++ b/jsk_2016_01_baxter_apc/test/test-ik.l @@ -0,0 +1,103 @@ +#!/usr/bin/env roseus +(require :unittest "lib/llib/unittest.l") +(require "package://jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter.l") +(load "package://jsk_2015_05_baxter_apc/euslisp/test/data/bin-cubes.l") + +(init-unit-test) + +(defun setup () + (unless (boundp '*baxter*) + (setq *baxter* (instance jsk_2016_01_baxter_apc::baxter-robot :init))) + (objects (list *baxter*)) + (send *baxter* :angle-vector #f(0.0 97.4707 -2.39502 -94.5483 134.67 91.4062 8.70117 0.0 -5.40301 55.9257 117.063 72.4793 -62.7243 -7.73117 -87.322 90.0)) + ) + +(defun check-result (av1 av2) + (let () + (when (and av1 av2) + (format t "av1 = ~A~%" av1) + (format t "av2 = ~A~%" av2) + (format t "diff = ~A (~A)~%" (v- av2 av1) (norm (v- av2 av1))) + (do ((i 0 (incf i 0.1))) + ((>= i 1)) + (send *baxter* :angle-vector (midpoint i av1 av2)) + (send *irtviewer* :draw-objects) + (print i)) + ))) + +(deftest issue-1474 () ;; https://github.com/start-jsk/jsk_apc/issues/1470#issuecomment-220518802 + (let (av1 av2) + (setup) + (setq av1 (send *baxter* :angle-vector)) + (send *baxter* :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)) + )) + +;; https://github.com/euslisp/jskeus/pull/380 +;; https://github.com/euslisp/jskeus/commit/9766a65d8346a939c9af89461691ccea334b2b7b#commitcomment-17593228 +(load "irteus/irtrobot.l") +(defmethod robot-model + (:calc-inverse-jacobian + (jacobi &rest args + &key + ((:manipulability-limit ml) 0.1) + ((:manipulability-gain mg) 0.001) + weight debug-view + ret wmat tmat umat umat2 mat-tmp + mat-tmp-rc tmp-mrr tmp-mrr2 + &allow-other-keys) + (let (jacobi# m m2 (k 0)) + ;; m : manipulability + (setq m (manipulability (if weight (m* jacobi (diagonal weight)) jacobi) tmp-mrr tmat)) ;; CHANGED + (if (< m ml) (setq k (* mg (expt (- 1.0 (/ m ml)) 2)))) + (when (and debug-view (not (memq :no-message debug-view))) + (warn "k :~7,3f (manipulability:~7,3f, gain:~7,3f, limit:~7,3f, len:~d)~%" k m mg ml (cadr (array-dimensions jacobi)))) + + ;; calc weighted SR-inverse + (setq jacobi# (sr-inverse jacobi k weight + ret wmat tmat umat umat2 mat-tmp + mat-tmp-rc tmp-mrr tmp-mrr2 + )) + jacobi#)) +) +;; check why normal ik failed +(deftest check-normal-ik-1 () ;; this uses exactry the same parameter with baxter-robot :inverse-kinematics in baxtereus/baxter-util.l + (let (av1 av2) + (setup) + (setq av1 (send *baxter* :angle-vector)) + (send *irtviewer* :draw-objects) + (setq av2 (send-message *baxter* robot-model :inverse-kinematics + (send (send *baxter* :rarm :end-coords :copy-worldcoords) :translate #f(0 0 -50) :local) + :additional-weight-list (mapcar #'(lambda (jn) (list (send *baxter* jn :child-link) 0)) + '(:right_gripper_vacuum_pad_joint)) + :move-target (send *baxter* :rarm :end-coords) + :link-list (send *baxter* :link-list (send *baxter* :rarm :end-coords :parent) (send *baxter* :rarm :root-link)) + :avoid-collision-distance 5 + :debug-view nil ;; set true for debugging + )) + (check-result av1 av2) + (assert (and av1 av2)) ;; this is expected to fail without new irteus + )) + +(deftest check-normal-ik-2 () ;; this may fix the problem + (let (av1 av2) + (setup) + (setq av1 (send *baxter* :angle-vector)) + (send *irtviewer* :draw-objects) + (setq av2 (send-message *baxter* robot-model :inverse-kinematics + (send (send *baxter* :rarm :end-coords :copy-worldcoords) :translate #f(0 0 -50) :local) + :move-target (send *baxter* :rarm :end-coords) + ;;:link-list (send *baxter* :link-list (send *baxter* :rarm :end-coords :parent) (send *baxter* :rarm :root-link)) + :link-list (send *baxter* :link-list (send *baxter* :right_gripper_vacuum_pad_joint :parent-link) (send *baxter* :rarm :root-link)) + :avoid-collision-distance 5 + :debug-view nil ;; set true for debugging + )) + (check-result av1 av2) + (assert (and av1 av2)) + )) + +(run-all-tests) +(exit) diff --git a/jsk_2016_01_baxter_apc/test/test_ik.test b/jsk_2016_01_baxter_apc/test/test_ik.test new file mode 100644 index 000000000..7edae98b3 --- /dev/null +++ b/jsk_2016_01_baxter_apc/test/test_ik.test @@ -0,0 +1,9 @@ + + + + + + +