search

start-jsk / jsk_apc

Other
36 stars 35 forks source link

arm moves with jerky start in case of useing moveit! #1989

Closed knorth55 closed 7 years ago

knorth55 commented 7 years ago

What happens

/robot/limb/right/joint_command/command is not so different, but /robot/joint_states is quite different. this problem occurs when I use :angle-vector-motion-plan

Graphs

when I move arm with moveit! moveit_joints_plot

at 3216, arm moves instead command is static

This is graph when I move arm without moveit! (default :angle-vector-raw) euslisp_joints_plot

Videos

Motion with Moveit!: https://drive.google.com/open?id=0B5DV6gwLHtyJSXBuc212a0wxVFU

Motion without Moveit!: https://drive.google.com/open?id=0B5DV6gwLHtyJYzBvaUlJT3Q1RHM

Motion planned by Moveit! (rviz) : https://drive.google.com/open?id=0B5DV6gwLHtyJNXN0aFl3dkRmR1k

Trajectory

This is /robot/limb/right/follow_joint_trajectory/goal topic with moveit!: https://gist.github.com/knorth55/c6e74ecc7aac21a1bc0724a52e496924 This is /robot/limb/right/follow_joint_trajectory/goal topic without moveit!: https://gist.github.com/knorth55/cd57bc8d2ab23a51eaa8dd428fe12d81

Info

I use baxter_interface with minjerk and arm moves with no jerky motion with euslisp default:angle-vector-raw. I checked with rviz and gazebo simulation, and planned trajectory works fine in both rviz and gazebo simulator.

@k-okada I have no idea what cause this problem. Could you help me?

knorth55 commented 7 years ago

This motion is done as below

$ roseus jsk_2016_01_baxter_apc/baxter-interface.l
configuring by "/home/knorth55/jskeus/eus/lib/eusrt.l"
;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin
connected to Xserver DISPLAY=:0
X events are being asynchronously monitored.
;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtpointcloud irtx eusjpeg euspng png irtimage irtglrgb
;; extending gcstack 0x6064700[16374] --> 0x64c6fb0[32748] top=3cc1
irtgl irtglc irtviewer
EusLisp 9.21(13f0443 1124dd2) for Linux64 created on sheeta(Fri Nov 18 17:54:58 JST 2016)
roseus ;; loading roseus("") on euslisp((9.21 sheeta Fri Nov 18 17:54:58 JST 2016 13f0443 1124dd2))
eustf roseus_c_util
;; extending gcstack 0x64c6fb0[32738] --> 0x6f49080[65476] top=7fe1
1.irteusgl$ (jsk_2016_01_baxter_apc::baxter-init :moveit t)
2.irteusgl$ (send *ri* :recognize-bin-boxes)

3.irteusgl$ (send *ri* :angle-vector (send *ri* :ik->bin-entrance :rarm :b :offset #f(-150 0 30)) :gripper-angle 90) 3000 :rarm-controller 0 :move-arm :rarm)
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
[:ik->nearest-pose] arm::rarm midpose: #f(80.0021 -19.7754 0.571289 -1.03271 90.9448 90.1099 -3.66943 -0.219727 0.0 -37.4634 -29.3774 142.449 117.883 82.0679 -18.8306 -123.882 90.0)
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
[ INFO] [1482589393.576254735]: gripper-angle: 90
[:ik->nearest-pose] arm::rarm midpose: #f(80.0021 -19.7754 0.571289 -1.03271 90.9448 90.1099 -3.66943 -0.219727 0.0 -37.4634 -29.3774 142.449 117.883 82.0679 -18.8306 -123.882 90.0)
[ INFO] [1482589393.700063915]: ;; Planned Trajectory Total Time   1.361 [sec]
[ INFO] [1482589393.700514933]: ;; Scaled Trajectory Total Time   3.000 [sec]
[ INFO] [1482589393.700571590]: ;; generated 10 points for 3 sec using [right_arm] group
(#f(80.0021 -19.7754 0.571289 -1.03271 90.9448 90.1099 -3.66943 -0.219727 0.0 -28.7442 -75.752 127.861 73.069 103.517 -41.8576 -126.333 90.0))

4.irteusgl$ (send *ri* :angle-vector (send *ri* :ik->bin-entrance :rarm :b :offset #f(-20 0 30)) :gripper-angle 90) 3000 :rarm-controller 0 :move-arm :rarm)
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
[:ik->nearest-pose] arm::rarm midpose: #f(80.0021 -19.7754 0.571289 -1.03271 90.9448 90.1099 -3.66943 -0.219727 0.0 -37.4634 -29.3774 142.449 117.883 82.0679 -18.8306 -123.882 90.0)
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
[ INFO] [1482589616.660738199]: gripper-angle: 90
[:ik->nearest-pose] arm::rarm midpose: #f(80.0021 -19.7754 0.571289 -1.03271 90.9448 90.1099 -3.66943 -0.219727 0.0 -37.4634 -29.3774 142.449 117.883 82.0679 -18.8306 -123.882 90.0)
[ INFO] [1482589616.768746888]: ;; Planned Trajectory Total Time   1.363 [sec]
[ INFO] [1482589616.769228746]: ;; Scaled Trajectory Total Time   3.000 [sec]
[ INFO] [1482589616.769285474]: ;; generated 10 points for 3 sec using [right_arm] group
(#f(80.0021 -19.7754 0.571289 -1.03271 90.9448 90.1099 -3.66943 -0.219727 0.0 7.74396 -74.462 85.7146 60.3092 109.939 -36.0376 -135.932 90.0))
pazeshun commented 7 years ago

/robot/limb/right/joint_command/commandは関節指令をbaxter内部のrealtime loopに送る最も下のレイヤーのtopicで、/robot/joint_statesもrealtime loopから直接流れてきているものなので、問題はもはやMoveitではなく、Baxter内部の処理にあるように思えます。 グラフを見てると、Moveitを使っても使わなくても、あまりtrajectoryが変わらないように見えるので、ますます不思議です。 めんどくさいとは思うのですが、この動きを左腕でやってみるとどうなるでしょうか。

pazeshun commented 7 years ago

@knorth55 が使用する前に、私がgripper-v5用に右腕の重力補償を以下のようにグリッパ重量を考慮したものに書き換えていました。（http://sdk.rethinkrobotics.com/wiki/Gripper_Customization#Gripper_and_Object_Mass）

$ rostopic pub -1 /robot/end_effector/right_gripper/command baxter_core_msgs/EndEffectorCommand '{ id : 131073, command : "configure", args : "{ \"urdf\":{ \"name\": \"right_gripper_mass\", \"link\": [ { \"name\": \"right_gripper_mass\", \"inertial\": { \"mass\": { \"value\": 1.18 }, \"origin\": { \"xyz\": [0.0, 0.0, 0.15] } } } ] }}"}'

使用後に以下のようにグリッパ重量を考慮しないものに書き換え直したのですが、

$ rostopic pub -1 /robot/end_effector/right_gripper/command baxter_core_msgs/EndEffectorCommand '{ id : 131073, command : "configure", args : "{ \"urdf\":{ \"name\": \"right_gripper_mass\", \"link\": [ { \"name\": \"right_gripper_mass\", \"inertial\": { \"mass\": { \"value\": 0 }, \"origin\": { \"xyz\": [0.0, 0.0, 0.0] } } } ] }}"}'

実は上の値は初期値ではなく、このせいで重力補償に異常が発生している・・・のかもしれません。ありえないとは思いますが、念の為書いておきます。

pazeshun commented 7 years ago

動画見て、やっぱり重力補償の問題ではないと思いました。 baxter_interfaceパッケージの https://github.com/wkentaro/baxter_interface/pull/1 をrevertしてみると治ったりするでしょうか。 これも結局/robot/limb/right/joint_commandに指令を送る前段階の話で、本当に3216秒目の飛躍が問題ならrevertしても意味ないようには感じるのですが・・・。

k-okada commented 7 years ago

この時のjointtrajectoryactionを教えてくれると嬉しいです． わからないけど、tm=0付近で変な位置をあたえていたり、速度が入っている可能性があるあｋな．と．

-- ◉ Kei Okada

2016-12-25 1:24 GMT+09:00 Shun Hasegawa notifications@github.com:

動画見て、やっぱり重力補償の問題ではないと思いました。 baxter_interfaceパッケージの wkentaro/baxter_interface#1 https://github.com/wkentaro/baxter_interface/pull/1 をrevertしてみると治ったりするでしょうか。

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/start-jsk/jsk_apc/issues/1989#issuecomment-269090708, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3DWHz0T90BH_qNbFw4e7OggX6k00ks5rLUdFgaJpZM4LVPMI .

pazeshun commented 7 years ago

この時のjointtrajectoryactionを教えてくれると嬉しいです． わからないけど、tm=0付近で変な位置をあたえていたり、速度が入っている可能性があるあｋな．と．

This is /robot/limb/right/follow_joint_trajectory/goal topic with moveit!: https://gist.github.com/knorth55/c6e74ecc7aac21a1bc0724a52e496924

を見ると、速度のlistは空になっているように見えます。 そして、グラフを見ると、joint_commandは滑らかにつながっているように見えます。 実は滑らかじゃないのかもしれませんが・・・。

knorth55 commented 7 years ago

joint_trajectoryのgoalはechoしたものをgistにおきました

This is /robot/limb/right/follow_joint_trajectory/goal topic with moveit!: https://gist.github.com/knorth55/c6e74ecc7aac21a1bc0724a52e496924

mmurooka commented 7 years ago

最初の時間 https://gist.github.com/knorth55/c6e74ecc7aac21a1bc0724a52e496924#file-joint_trajectory_with_moveit-txt-L28-L29 が怪しいかも． https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L682 が参考になるかも． by @k-okada

mmurooka commented 7 years ago 
[http://baxter:11311][133.11.216.159] leus@cygnus:~$ rostopic hz /robot/limb/right/joint_command
subscribed to [/robot/limb/right/joint_command]
average rate: 92.753
    min: 0.010s max: 0.012s std dev: 0.00045s window: 89
average rate: 93.081
    min: 0.010s max: 0.012s std dev: 0.00043s window: 182

でした．

knorth55 commented 7 years ago

:send-trajectoryの:starttime (default: 1.0)と:angle-vector-motion-planのstart-offset-timeの間に乖離がある場合に発生するバグと思われます https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/276 を使用してjerky motionが解消されたことを確認しました 下のプロットが https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/276 を使用した場合で、euslispの場合と同じようななめらかな動きになりました moveit_joints_plot_debugged

knorth55 commented 7 years ago

このときの/robot/limb/right/follow_joint_trajectory/goalはhttps://gist.github.com/knorth55/50413fad9b0966abd302bfc38021bb76のようになりました