From e9b67f658b578da9204aedc2495af9f0b07c801a Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Sat, 30 Apr 2016 16:43:37 +0900 Subject: [PATCH 1/5] Test move-arm-to-bin with rosbag for bin boxes --- jsk_2016_01_baxter_apc/CMakeLists.txt | 7 ++++++- jsk_2016_01_baxter_apc/package.xml | 1 + .../scripts/download_test_data.sh | 5 +++++ .../test/test-move-arm-to-bin.l | 16 ++++++++++++++++ .../test/test_move_arm_to_bin.test | 13 +++++++++++++ jsk_2016_01_baxter_apc/test_data/.gitignore | 2 ++ 6 files changed, 43 insertions(+), 1 deletion(-) create mode 100755 jsk_2016_01_baxter_apc/scripts/download_test_data.sh create mode 100755 jsk_2016_01_baxter_apc/test/test-move-arm-to-bin.l create mode 100644 jsk_2016_01_baxter_apc/test/test_move_arm_to_bin.test create mode 100644 jsk_2016_01_baxter_apc/test_data/.gitignore diff --git a/jsk_2016_01_baxter_apc/CMakeLists.txt b/jsk_2016_01_baxter_apc/CMakeLists.txt index 42fbc3b6c..9d5dead96 100644 --- a/jsk_2016_01_baxter_apc/CMakeLists.txt +++ b/jsk_2016_01_baxter_apc/CMakeLists.txt @@ -9,7 +9,6 @@ find_package(catkin REQUIRED COMPONENTS control_msgs roscpp std_msgs - rostest baxter_core_msgs ) @@ -230,5 +229,11 @@ add_custom_target(compile_stow_kiva_world ALL ## Add folders to be run by python nosetests if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) add_rostest(test/work_order.test) + add_custom_target(download_test_data COMMAND ${PROJECT_SOURCE_DIR}/scripts/download_test_data.sh) + if(TARGET tests) + add_dependencies(tests download_test_data) + endif() + add_rostest(test/test_move_arm_to_bin.test) endif() diff --git a/jsk_2016_01_baxter_apc/package.xml b/jsk_2016_01_baxter_apc/package.xml index fee32a56f..149944637 100644 --- a/jsk_2016_01_baxter_apc/package.xml +++ b/jsk_2016_01_baxter_apc/package.xml @@ -38,6 +38,7 @@ roscpp std_msgs + python-gdown rostest jsk_tools diff --git a/jsk_2016_01_baxter_apc/scripts/download_test_data.sh b/jsk_2016_01_baxter_apc/scripts/download_test_data.sh new file mode 100755 index 000000000..ece8cdd0e --- /dev/null +++ b/jsk_2016_01_baxter_apc/scripts/download_test_data.sh @@ -0,0 +1,5 @@ +#!/usr/bin/env bash + +DATA_DIR=$(rospack find jsk_2016_01_baxter_apc)/test_data + +gdown "https://drive.google.com/uc?id=0B9P1L--7Wd2vZ2xLZG55OWNYTDQ" -O $DATA_DIR/2016-04-30-16-33-54_apc2016-bin-boxes.bag diff --git a/jsk_2016_01_baxter_apc/test/test-move-arm-to-bin.l b/jsk_2016_01_baxter_apc/test/test-move-arm-to-bin.l new file mode 100755 index 000000000..f996d0fb8 --- /dev/null +++ b/jsk_2016_01_baxter_apc/test/test-move-arm-to-bin.l @@ -0,0 +1,16 @@ +#!/usr/bin/env roseus + +(require :unittest "lib/llib/unittest.l") +(require :baxter-interface "package://jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l") + +(init-unit-test) + +(deftest + test-move-arm-to-bin + (jsk_2016_01_baxter_apc::baxter-init) + (send *ri* :recognize-bin-boxes :stamp (ros::time 0)) + (assert (send *ri* :ik->bin-entrance :rarm :e)) + ) + +(run-all-tests) +(exit) diff --git a/jsk_2016_01_baxter_apc/test/test_move_arm_to_bin.test b/jsk_2016_01_baxter_apc/test/test_move_arm_to_bin.test new file mode 100644 index 000000000..01c219f15 --- /dev/null +++ b/jsk_2016_01_baxter_apc/test/test_move_arm_to_bin.test @@ -0,0 +1,13 @@ + + + + + + + + + diff --git a/jsk_2016_01_baxter_apc/test_data/.gitignore b/jsk_2016_01_baxter_apc/test_data/.gitignore new file mode 100644 index 000000000..d6b7ef32c --- /dev/null +++ b/jsk_2016_01_baxter_apc/test_data/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore From 0fa32375476ec2f58bbf179a17ad821479e27e4d Mon Sep 17 00:00:00 2001 From: pazeshun Date: Sat, 30 Apr 2016 17:23:41 +0900 Subject: [PATCH 2/5] apply test to every bin --- jsk_2016_01_baxter_apc/test/test-move-arm-to-bin.l | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/jsk_2016_01_baxter_apc/test/test-move-arm-to-bin.l b/jsk_2016_01_baxter_apc/test/test-move-arm-to-bin.l index f996d0fb8..d2f03453f 100755 --- a/jsk_2016_01_baxter_apc/test/test-move-arm-to-bin.l +++ b/jsk_2016_01_baxter_apc/test/test-move-arm-to-bin.l @@ -9,7 +9,18 @@ test-move-arm-to-bin (jsk_2016_01_baxter_apc::baxter-init) (send *ri* :recognize-bin-boxes :stamp (ros::time 0)) - (assert (send *ri* :ik->bin-entrance :rarm :e)) + (dolist (bin (list :a :b :c :d :e :f :g :h :i :j :k :l)) + (case bin + ((:a :d :g :j) (setq arm-list (list :larm))) + ((:b :e :h :k) (setq arm-list (list :larm :rarm))) + ((:c :f :i :l) (setq arm-list (list :rarm))) + ) + (dolist (arm arm-list) + (dolist (offset (list #f(-150 0 0) #f(-50 0 0) #f(0 0 0) #f(0 0 -80))) + (assert (send *ri* :ik->bin-entrance arm bin :offset offset)) + ) + ) + ) ) (run-all-tests) From b316a9dd1bdfaf830e7643b801e53add2d1f8bd7 Mon Sep 17 00:00:00 2001 From: pazeshun Date: Thu, 28 Apr 2016 10:54:04 +0900 Subject: [PATCH 3/5] don't use gripper joint to solve ik --- .../euslisp/jsk_2016_01_baxter_apc/baxter.l | 10 ++++++++++ 1 file changed, 10 insertions(+) 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 5fcf21d4a..8efa730c5 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 @@ -13,6 +13,16 @@ (&rest args) (send-super* :init args) ) + (: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))) + &allow-other-keys) + (send-super* :inverse-kinematics target-coords + :additional-weight-list additional-weight-list args) + ) (:rotate-wrist (arm angle) (let (wrist-index av-tmp) From a2be0f85e9a76a4f70247b4c36256d9eed066647 Mon Sep 17 00:00:00 2001 From: pazeshun Date: Thu, 28 Apr 2016 14:20:39 +0900 Subject: [PATCH 4/5] change middle point of ik --- jsk_2016_01_baxter_apc/robots/baxter.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/jsk_2016_01_baxter_apc/robots/baxter.yaml b/jsk_2016_01_baxter_apc/robots/baxter.yaml index 4655c914c..6b4c2cd80 100644 --- a/jsk_2016_01_baxter_apc/robots/baxter.yaml +++ b/jsk_2016_01_baxter_apc/robots/baxter.yaml @@ -27,9 +27,9 @@ angle-vector: reset-manip-pose : [0, -5, -57, -68, 111, 38, 59, 0, 5, -57, 68, 111, -38, 59, 0, 0] untuck-pose : [0, -5, -57, -68, 111, 38, 59, 0, 5, -57, 68, 111, -38, 59, 0, 0] # this is reset-manip-pose tuck-pose : [0, -57, -119, 152, 146, 0, 0, 0, 57, -119, -152, 136, 0, 0, 0, 0] - fold-pose-upper: [0, -7.80029, -1.12061, -82.9248, 141.438, 116.477, -6.48193, 8.10791, 7.80029, -1.12061, 82.9248, 141.438, -116.477, -6.48193, -8.10791, 0] - fold-pose-middle: [0, 48.8672, -10.437, -80.6616, 148.645, 80.9033, 1.38428, 18.8745, -48.8672, -10.437, 80.6616, 148.645, -80.9033, 1.38428, -18.8745, 0] - fold-pose-lower: [0, -7.27295, 52.6465, -59.9414, 91.582, 123.574, 13.3374, 159.675, 7.27295, 52.6465, 59.9414, 91.582, -123.574, 13.3374, -159.675, 0] + fold-pose-upper: [0, -7.80029, -1.12061, -82.9248, 141.438, 116.477, -6.48193, 8.10791, 35.6955, -0.550063, 174.754, 89.2461, -169.141, 85.136, 4.69734, 90] # this pose of rarm is symmetrical to larm on entrance of bin a + fold-pose-middle: [0, 48.8672, -10.437, -80.6616, 148.645, 80.9033, 1.38428, 18.8745, -48.8672, -10.437, 80.6616, 148.645, -80.9033, 1.38428, -18.8745, 90] + fold-pose-lower: [0, -7.27295, 52.6465, -59.9414, 91.582, 123.574, 13.3374, 159.675, 7.27295, 52.6465, 59.9414, 91.582, -123.574, 13.3374, -159.675, 90] fold-pose-back: [0, 97.4707, -2.39502, -94.5483, 134.67, 91.4062, 8.70117, 0.0, -97.4707, -2.39502, 94.5483, 134.67, -91.4062, 8.70117, 0.0, 0] fold-to-keep-object: [0, 77.1899, 4.37256, -86.1548, 123.904, 11.8213, 59.8096, 74.5532, -77.1899, 4.37256, 86.1548, 123.904, -11.8213, 59.8096, -74.5532, 90] place-object-pose: [0, 5.07568, -13.6011, -44.7803, 101.184, -0.725098, 6.63574, 88.6597, -5.07568, -13.6011, 44.7803, 101.184, 0.725098, 6.63574, -88.6597, 0] From 22a94b16537997cdfa160a36929e597f9ac0bd5e Mon Sep 17 00:00:00 2001 From: pazeshun Date: Thu, 28 Apr 2016 17:00:34 +0900 Subject: [PATCH 5/5] rotate gripper joint by script not by ik --- .../euslisp/jsk_2016_01_baxter_apc/baxter-interface.l | 6 ++---- .../euslisp/jsk_2016_01_baxter_apc/baxter.l | 10 +++------- 2 files changed, 5 insertions(+), 11 deletions(-) diff --git a/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l b/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l index 599968e52..f7eda95c4 100644 --- a/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l +++ b/jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l @@ -368,15 +368,13 @@ (send bin-coords :translate (float-vector (- (/ bin-dim-x 2)) 0 0) :world) (cond ((< sign_y 0) (send end-coords :move-to (send bin-coords :worldcoords) :world) - (send end-coords :rotate (* sign_y pi/2) :y :local)) + (send end-coords :rotate (* sign_y pi/2) :y :local) + (send *baxter* :rotate-gripper :rarm 0 :relative nil)) (t (send bin-coords :assoc end-coords) (send bin-coords :rotate (* sign pi/2) :x :local) (send bin-coords :dissoc end-coords)) ) - (if (< sign 0) - (send *baxter* :rotate-wrist arm 270) - ) (pushback (send *baxter* arm :inverse-kinematics end-coords 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 8efa730c5..ac96eaeec 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 @@ -32,14 +32,10 @@ (send self :angle-vector av-tmp)) ) (:rotate-gripper - (arm angle) + (arm angle &key (relative t)) (if (eq arm :rarm) - (let (gripper-index av-tmp) - (setq gripper-index 15) - (setq av-tmp (send self :angle-vector)) - (setf (elt av-tmp gripper-index) (+ (elt av-tmp gripper-index) angle)) - (send self :angle-vector av-tmp)) - ) + (send self arm :gripper-p :joint-angle angle :relative relative)) + (send self :angle-vector) ) (:avoid-shelf-pose (arm bin)