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

[Depends on #1378] [jsk_2016_01_baxter_apc] don't use gripper joint to solve IK #1362

Merged
merged 5 commits into from
Apr 30, 2016
Merged

[Depends on #1378] [jsk_2016_01_baxter_apc] don't use gripper joint to solve IK #1362

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
e9b67f6
Test move-arm-to-bin with rosbag for bin boxes
wkentaro Apr 30, 2016
0fa3237
apply test to every bin
pazeshun Apr 30, 2016
b316a9d
don't use gripper joint to solve ik
pazeshun Apr 28, 2016
a2be0f8
change middle point of ik
pazeshun Apr 28, 2016
22a94b1
rotate gripper joint by script not by ik
pazeshun Apr 28, 2016
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
7 changes: 6 additions & 1 deletion jsk_2016_01_baxter_apc/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@ find_package(catkin REQUIRED COMPONENTS
control_msgs
roscpp
std_msgs
rostest
baxter_core_msgs
)

Expand Down Expand Up @@ -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()
6 changes: 2 additions & 4 deletions jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter-interface.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
20 changes: 13 additions & 7 deletions jsk_2016_01_baxter_apc/euslisp/jsk_2016_01_baxter_apc/baxter.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -22,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)
Expand Down
1 change: 1 addition & 0 deletions jsk_2016_01_baxter_apc/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>

<test_depend>python-gdown</test_depend>
<test_depend>rostest</test_depend>
<test_depend>jsk_tools</test_depend>
</package>
6 changes: 3 additions & 3 deletions jsk_2016_01_baxter_apc/robots/baxter.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

これを変えても、move-arm-body->binメソッドなどは問題ない?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

この中間姿勢は、ik->bin-entranceメソッドでしか使用してなかったはずです。

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

特に、fold-pose-upperについては、変えないとIK普通に解いたときに手首の関節が可動域ギリギリの姿勢が出てきてしまい、そのあとのIKが失敗します。

Copy link
Member

@wkentaro wkentaro Apr 29, 2016 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

  • :ik->bin-entranceでIKの初期姿勢になっている
  • :ik->bin-entranceは他のメソッドで使われている

というのが問題かと思います．

Copy link
Collaborator Author

@pazeshun pazeshun Apr 29, 2016 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

今問題になっているのは、ik->bin-entranceで出てくる姿勢がおかしいということです。
それを修正するために、中間姿勢を修正しました。
この中間姿勢はあくまで*baxter*に送られるものであり、これで実機が動くわけではないと考えています。

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

このPRの副作用を調べるためなので、このPRに必要ですね。

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

わかりました。
といっても、なんのテストを作ればいいのかよくわかっていないのですが、ik->bin-entranceが成功するかどうかをテストすればいいのでしょうか。それとも、よりシーケンシャルな動き（例えば、:move-arm-body->binメソッドをしてから、ik->bin-entranceを使用している各オフセットで解き、それから:view-hand-pose のあと:move-arm-body->order-bin）をテストすれば良いのでしょうか。

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

変更したfold-poseを使っているのは:ik->bin-entranceだけだったということなので、:ik->bin-entranceだけでいいんじゃないでしょうか。(引数をいくつか変えて）

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

わかりました。
#1367 のようなものをik->bin-entranceに対して作ります。できたらmergeしてしまおうと思っています。
大丈夫でしょうか？

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

できたら@ban-masaに一度見てもらって下さい．

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]
Expand Down
5 changes: 5 additions & 0 deletions jsk_2016_01_baxter_apc/scripts/download_test_data.sh
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
27 changes: 27 additions & 0 deletions jsk_2016_01_baxter_apc/test/test-move-arm-to-bin.l
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
#!/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))
(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)
(exit)
13 changes: 13 additions & 0 deletions jsk_2016_01_baxter_apc/test/test_move_arm_to_bin.test
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
<launch>

<env name="DISPLAY" value="" />

<node name="rosbag_play"
pkg="rosbag" type="play"
args="$(find jsk_2016_01_baxter_apc)/test_data/2016-04-30-16-33-54_apc2016-bin-boxes.bag --clock" />

<test test-name="test_move_arm_to_bin"
name="test_move_arm_to_bin"
pkg="jsk_2016_01_baxter_apc" type="test-move-arm-to-bin.l" />

</launch>
2 changes: 2 additions & 0 deletions jsk_2016_01_baxter_apc/test_data/.gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
*
!.gitignore