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

knorth55 · 2016-12-24T14:35:01Z

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!

at 3216, arm moves instead command is static

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

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?

The text was updated successfully, but these errors were encountered:

knorth55 · 2016-12-24T14:37:30Z

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 · 2016-12-24T16:03:41Z

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

pazeshun · 2016-12-24T16:10:15Z

@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 · 2016-12-24T16:24:36Z

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

k-okada · 2016-12-25T14:59:12Z

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

…

-- ◉ Kei Okada 2016-12-25 1:24 GMT+09:00 Shun Hasegawa <[email protected]>:

動画見て、やっぱり重力補償の問題ではないと思いました。 baxter_interfaceパッケージの wkentaro/baxter_interface#1 <wkentaro/baxter_interface#1> をrevertしてみると治ったりするでしょうか。 — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#1989 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AAeG3DWHz0T90BH_qNbFw4e7OggX6k00ks5rLUdFgaJpZM4LVPMI> .

pazeshun · 2016-12-25T15:15:11Z

この時の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 · 2016-12-25T17:10:43Z

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

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

mmurooka · 2016-12-27T07:33:40Z

最初の時間
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 · 2016-12-27T07:36:22Z

[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 · 2016-12-27T13:11:25Z

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

knorth55 · 2016-12-27T13:17:35Z

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

knorth55 added bug pkg/jsk_2016_01_baxter_apc labels Dec 24, 2016

knorth55 self-assigned this Dec 24, 2016

knorth55 mentioned this issue Dec 27, 2016

[pr2eus_moveit] pass start-offset-time as starttime to :send-trajectory jsk-ros-pkg/jsk_pr2eus#276

Merged

knorth55 closed this as completed Dec 27, 2016

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

knorth55 commented Dec 24, 2016 •

edited

Loading

knorth55 commented Dec 24, 2016 •

edited

Loading

pazeshun commented Dec 24, 2016

pazeshun commented Dec 24, 2016 •

edited

Loading

pazeshun commented Dec 24, 2016 •

edited

Loading

k-okada commented Dec 25, 2016 via email •

edited

Loading

pazeshun commented Dec 25, 2016

knorth55 commented Dec 25, 2016

mmurooka commented Dec 27, 2016

mmurooka commented Dec 27, 2016

knorth55 commented Dec 27, 2016

knorth55 commented Dec 27, 2016

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

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

Comments

knorth55 commented Dec 24, 2016 • edited Loading

What happens

Graphs

Videos

Trajectory

Info

knorth55 commented Dec 24, 2016 • edited Loading

pazeshun commented Dec 24, 2016

pazeshun commented Dec 24, 2016 • edited Loading

pazeshun commented Dec 24, 2016 • edited Loading

k-okada commented Dec 25, 2016 via email • edited Loading

pazeshun commented Dec 25, 2016

knorth55 commented Dec 25, 2016

mmurooka commented Dec 27, 2016

mmurooka commented Dec 27, 2016

knorth55 commented Dec 27, 2016

knorth55 commented Dec 27, 2016

knorth55 commented Dec 24, 2016 •

edited

Loading

knorth55 commented Dec 24, 2016 •

edited

Loading

pazeshun commented Dec 24, 2016 •

edited

Loading

pazeshun commented Dec 24, 2016 •

edited

Loading

k-okada commented Dec 25, 2016 via email •

edited

Loading