inverse-kinematicsで手首が360度回転してしまう #87

k-okada · 2014-03-03T18:24:55Z

This post was originally posted at http://sourceforge.net/p/jsk-ros-pkg/tickets/91

k-okada · 2014-03-03T18:38:08Z

この現象が生じるサンプルコードを添付するか，あるいはjsk_2013_04_pr2_610のどこかのコードの関数を実行すると，この現象になるとか，教えてください．

k-okada · 2014-03-03T18:38:09Z

assigned_to: Kei Okada

k-okada · 2014-03-03T18:38:09Z

以下のコードで発生しました。
確認したところ、360度回転が起きているところはinverse-kinematicsではなくmove-end-posの部分でした。
またinverse-kinematicsのz-axisには0か180の値が入ります。

(send pr2 :reset-pose)
(send pr2 :inverse-kinematics
(make-coords :pos (send pr2 :transform-vector #f(800 600 900)) :rpy (float-vector 0 0 (deg2rad z-axis)))
:rotation-axis t
:move-target (send pr2 :larm :end-coords)
:use-torso t
:debug-view nil)
(send pr2 :head :look-at (send pr2 :larm :end-coords :worldpos))
(send ri :angle-vector (send pr2 :angle-vector))
(send ri :wait-interpolation)

(send ri :stop-grasp :larm)
(unix::sleep 2)
(send ri :wait-interpolation)
(send pr2 :larm :move-end-pos #f(-500 0 0))
(send ri :angle-vector (send pr2 :angle-vector))
(send ri :wait-interpolation)

k-okada · 2014-03-03T18:38:10Z

(load "package://pr2eus/pr2-interface.l")
(ros::roseus "hoge")

(setq pr2 (pr2))
(setq ri (instance pr2-interface :init))

(send pr2 :angle-vector #f(.....))
(send ri :angle-vector (send pr2 :angle-vector))
(send pr2 :larm :move-end-pos #f(-500 0 0))
(send ri :angle-vector (send pr2 :angle-vector))
(send ri :wait-interpolation)

みたいな１０行以内のサンプルで再現するコードは作れたりするかな？

k-okada · 2014-03-03T18:38:11Z

添付したコードで再現されるようです。
よろしくお願いします。

k-okada · 2014-03-03T18:38:12Z

これはかなり難しいですね．簡単な解決としては，
(dotimes (i 5)
(send pr2 :larm :move-end-pos #f(-100 0 0)
(send ri :angle-vector (send pr2 :angle-vector)))
などとして回避してください．

本格的な解決は少し考える時間が必要な気がします．

k-okada · 2014-03-03T18:38:12Z

[r4195]でpr2-interface.l直しました．pr2eusをsvn upして送ってくれたサンプルプログラム，並びに元のプログラムを動かしてください．治っていると思います．また副作用がある可能性もあるので，皆さん自分のプログラムを動かして書くにしてもらえると助かります．

pr2は無限回転のジョイントを幾つか持っている
eusでも:min-angle, :max-angle をみると_-inf_/_inf_としている
一方でeusではsend pr2 :angle-vectorとすると，-360/360以内のデータしか帰ってこないようにしている．
-実機のコントローラは無限回転の場合，最小パスを通るように回転方向を決めるようになっている

今回の例題では

ikを解いた時の姿勢A, 姿勢Bで手のひら（？）の向きに変更はなかった．
姿勢Aと姿勢Bを補完した場合，360度向きが変わることはなかった（simulation-modeで確認できる）
一方で実機におくると360度回転している．
これは姿勢Aと姿勢Bである関節が70度から-130度に変化しあていたこれは，70 -> 0 -> -130と遷移することが期待されていた
一方で実機は参照をパスを選択するため70 ->180/-180 -> -130と移動していた．

ということで，angle-vector-sequenceでは2つの指令値の差分をみて回転角度で180度を超えているところがあれば，その中間の指令値をangle-vector-sequenceの間に入れ込むという作業をしている．

angle-vectorの場合は最後の指令値を:reference-vectorで持ってきてこれと指令値の差分をみて180度を超えているか調べているが，この部分はちょっと心配．:potentio-vectorで持ってくると180になったり-180になる関節があり，360度回転する関節が出てくるため．

デバッグtips
動作を作り込む時，あるいはデバッグする時は

roslaunch pr2_gazebo pr2_empty_world.launch

とすると，実機と全く同じインターフェースをもつシミュレータが立ち上がるので大変便利

k-okada · 2014-03-03T18:38:13Z

返信が遅くなってしまいました。
pr2eus以下をsvn upして360-sample.lを動かして見ましたが、pr2-interface上では以前と同じように手首が360度回転してしまうようです。
実機ではまだ試していません。

また
(dotimes (i 5)
(send pr2 :larm :move-end-pos #f(-100 0 0)
(send ri :angle-vector (send pr2 :angle-vector)))
を使えば細切れにはなりますが、目的の動作を達成可能でした。

k-okada · 2014-03-03T18:38:14Z

なるほど，robot-interface.lの:anlge-vectorでは:sub-angle-vectorをつかって，無限回転の場合は近い方に動く（実機と同じ動きをする）ようになっていまいした．ということで，pr2-interfaceの:angle-vectorで，180度を超える動きをするときに:angle-vector-sequenceを使う，というコードをpr-2interfaceでも動くようにしました．[r4220]

k-okada · 2014-03-03T18:38:15Z

添付のコードで腕が回ってしまうことを確認しました。
angle-vectorを２回だと回りますが、
angle-vector-sequenceだと回らずに（正常に）動作しました。

k-okada · 2014-03-03T18:38:16Z

5/1日に報告してくれたtest.lは治っているはずです．また，
https://sourceforge.net/p/jsk-ros-pkg/tickets/145/
の変更でreset-poseが変わったので，このプログラムみたいに
angle-vectorが決め打ちなものは直す必要があります．
添付のがなっているとおもいますから，確認してください．

k-okada · 2014-03-03T18:38:16Z

添付のコードで手先がクルッとしてしまいます。

k-okada · 2014-03-03T18:38:17Z

test2.lは関節の肘ロールがリミットの180付近にあるのが問題のようです．
でよく考えてみると，そもそも無限回転の関節を+-180にしているのが
問題ではないかという事で　https://sourceforge.net/p/jskeus/tickets/25/　にあるように270にしました．
これで，[#145]は必要なくなったと思います．

なお，この問題と，そもそもこのチケットで問題になった事柄は微妙に違う問題です．

k-okada · 2014-03-03T18:38:18Z

move-toなどで移動中にも（PR2のangle-vectorに送っていなくても）手首が回転してしまう時があります。

k-okada · 2014-03-03T18:38:19Z

(defun hoge nil
  (send *ri* :angle-vector 
        #f(92.7342 37.2226 12.3315 46.6177 -44.4835 187.553 -30.9676 128.145 -30.7465 10.2422 -59.3904 -105.095 -89.2577 -28.9264 174.513 34.6816 27.3987))
  (send *ri* :wait-interpolation)
  (send *ri* :angle-vector
        #f(55.2304 122.301 14.1074 126.756 -121.311 -90.6499 -52.6751 34.2688 -30.7465 10.2422 -59.3904 -105.095 -89.2577 -28.9264 174.513 71.0005 52.6066))
  (send *ri* :wait-interpolation))

でダメな例があるようです。以下のようなメッセージはでているんだけど。。。

[ WARN] [1370679049.165107197]: continuous joint (l_forearm_roll_joint) moves -278.203 degree, comman\
ded joint differs from original trajectory to avoid unintentional 360 rotation
[ WARN] [1370679049.165305964]: original trajectory command :
[ WARN] [1370679049.165359749]:                        : #f(55.2304 122.301 14.1074 126.756 -121.311 \
-90.6499 -52.6751 34.2688 -30.7465 10.2422 -59.3904 -105.095 -89.2577 -28.9264 174.513 71.0005 52.606\
6) 3000
[ WARN] [1370679049.165387526]: new trajectory command :
[ WARN] [1370679049.165450168]:                        : #f(73.9823 79.7618 13.2194 86.6868 -82.8972 \
48.4515 -41.8213 81.2069 -30.7465 10.2422 -59.3904 -105.095 -89.2577 -28.9264 174.513 52.841 40.0026)\
 1500.0
[ WARN] [1370679049.165502122]:                        : #f(55.2304 122.301 14.1074 126.756 -121.311 \
-90.6499 -52.6751 34.2688 -30.7465 10.2422 -59.3904 -105.095 -89.2577 -28.9264 174.513 71.0005 52.606\
6) 1500.0

k-okada · 2014-03-03T18:38:19Z

another test code to check

(defun hoge nil
  (send *ri* :angle-vector
        #f(78.892 122.143 16.5724 128.784 -118.564 -86.9813 -53.4699 32.4022 -30.746 10.2413 -59.3886 -105.093 -89.2534 -28.9254 174.516 70.999 54.714))
  (send *ri* :wait-interpolation)
  (send *ri* :angle-vector
        #f(100.0 10.6748 2.30296 30.0196 -97.4113 107.346 -62.9762 171.355 -30.7465 10.2422 -59.3904 -105.095 -89.2577 -28.9264 174.513 -3.07712 54.4601))
  (send *ri* :wait-interpolation)
  )

k-okada · 2014-03-03T18:38:20Z

#f(167.707 29.4094 -11.5979 30.2555 -21.9039 170.135 -32.6374 158.616 -30.7465 10.2422 -59.3904 -105.095 -89.2577 -28.9264 174.513 34.683 15.5661)                                                         
#f(167.707 25.5906 28.6932 5.78789 -16.8607 -32.6516 -13.3686 26.8255 -30.7465 10.2422 -59.3904 -105.095 -89.2577 -28.9264 174.513 34.6855 39.3216)

k-okada · 2014-03-03T18:38:21Z

pr2-ri-test.l を追加しました．
関節角度av0とav1を送った時に手先がひっくり返るか，というのを調べていますが，
上で挙げたほどんどのケースはav0とav1を以下のように補間すると，

    (dotimes (i 10)                                                                                                                             
      (send *pr2* :angle-vector (midpoint (/ i 10.0) av0 av1)))

手がひっくり返るので，そもそもの指令値がいけない，ということになります．

そもそも，なぜひっくり返る指令が出るかという事ですが，

で，途中で関節がリミット（無限関節における270のリミット）を超えていますね．
そもそもはそこが問題かと思います．
https://sourceforge.net/p/jskeus/tickets/29/
にチケットを起きました．

k-okada · 2014-03-03T18:38:22Z

で，angle-vectorを送るときに，手先の座標系での補間する:angle-vector-with-constraintを作りました．[r4613], pr2-interface.lに定義していますが，robot-interface.lにいれるのでいいでしょうか？いまは:larm,:rarmになっていますが，:armsが必要か，とか，use-torsoはどうする？とか，unless joint-action-enableでいいのか？（(send self :simulation-modep)を使う？使い分けは？）などの疑問が開発者向けにはあります．

利用者の人はどうしても手が回ってしまう時に，
(send ri :angle-vector-with-constraint av1 3000 :larm :rotation-axis :z)
とすると上手く動くときもある，というのを覚えておいて下さい．

k-okada · 2014-03-03T18:38:23Z

:angle-vector-with-constraint ですが，[r4634]で:armsにも対応しました．

k-okada · 2014-03-03T18:38:23Z

https://sourceforge.net/p/jskeus/tickets/29/ #29 無限回転関節でのIKで，関節リミットを超える
https://sourceforge.net/p/jskeus/tickets/25/ #25 無限回転の関節リミットを180から広げる
[#188] pick-trayで手が回転する。

全て同じ問題ですが，復習すると，
eusでは無限界点関節のmin-angle/max-angelは_-inf_ _inf_になっている
一方関節は仮想的なリミットである180度（今は270度）で丸められる．つまり，
send j :joint-angle 300　とすると，joint-angle　は　-60になる．
IK自体は関節リミットを見ていない．なので，仮想的なリミットを超えてうごいている．これが
https://sourceforge.net/p/jskeus/tickets/29/ 　の問題．
これは添付した29.patchで解決できる．

実機の方の振る舞いとしては，実機の補間器は近い方の回転を選ぶ，つまり−180度の時に180度を
指令しても動かず，-170の時に170を指令すると-20動く．（これでいいかは疑問がかも）

昔は，_ri_は直せつ実機の補間機に送っていたが，
今は180度を超えて指令する時は途中で経由店を追加して-170->0->170として340度回転するようにしている．

昔の仕様を１），今の仕様を２）とすると，

１）昔の仕様だと180度を超えた指令をしないようにしないといけない．したがって，
eus側のIKで解けた180度を超えた動きをするけど，手のひらは回転しない指令を
実機におくっても，実機は回転していた．

２）今の仕様だと上の問題はなくなったが，今度はIkを説くときに180／−180の仮想リミットを
超えた指令が生成されているので，上の29.patchのような対応が必要になりそう．
ただ，これはIkで解ける範囲を狭めているのであまりよくない．

とはいえ１に戻ると，今度はやはり180度を超える問題は解決できない．ということで，
仮想リミットをやめて，無限関節は無限関節のまま扱うのがいい気がしてきました．
都の時は実機の補間器が値をそのまま解釈してくれるのが一番で，それがない間は
経由点を追加するバージョンを使う必要がある，というきがします．

つまり以下のような変更を加えるということです．これでいいような気がしますが
どうでしょうか？見落としてん，使いにくそうなどあれば教えて下さい．

k-okada@kokada-t430s:~/ros/fuerte/jsk-ros-pkg/euslisp/jskeus/irteus$ svn diff
Index: irtmodel.l
===================================================================
--- irtmodel.l  (リビジョン 922)
+++ irtmodel.l  (作業コピー)
@@ -156,16 +156,10 @@
    (let ()
      (when v
        (if relative (setq v (+ v joint-angle)))
-       (cond ((and (eq max-angle *inf*) (>= v 270.0))
-         (setq v (mod v 360))
-         (if (> v 270.0) (setq v (- v 360.0))))
-        ((> v max-angle)
+       (cond ((> v max-angle)
          (unless relative (warning-message 3 ";; ~A :joint-angle(~A) violate max-angle(~A)~%" self v max-angle))
          (setq v max-angle)))
-       (cond ((and (= min-angle *-inf*) (<= v -270.0))
-         (setq v (mod v 360))
-         (if (<= v -270.0) (setq v (+ v 360.0))))
-        ((< v min-angle)
+       (cond ((< v min-angle)
          (unless relative (warning-message 3 ";; ~A :joint-angle(~A) violate min-angle(~A)~%" self v min-angle))
          (setq v min-angle)))
        (setq joint-angle v)

k-okada · 2014-03-03T18:38:24Z

IRTデモを行ったところ不自然な回転はしないようになりました。

k-okada · 2014-03-03T18:38:25Z

status: open --> closed

[travis.sh] do not error when .rosinstall is not exists

k-okada self-assigned this Mar 3, 2014

k-okada closed this as completed Mar 3, 2014

YoheiKakiuchi mentioned this issue Mar 8, 2014

send :angle-vector arm rolling unintentionally jsk-ros-pkg/jsk_pr2eus#1

Open

wkentaro pushed a commit to wkentaro/jsk_common that referenced this issue Aug 11, 2016

Merge pull request jsk-ros-pkg#87 from k-okada/no_rosinstall

76e8a69

[travis.sh] do not error when .rosinstall is not exists

k-okada mentioned this issue Oct 1, 2017

add :wait-interpolation in test jsk-ros-pkg/jsk_robot#847

Merged

Naoki-Hiraoka mentioned this issue Dec 12, 2020

:angle-vectorと:potentio-vectorに差があると:angle-vector-sequenceの動き出しがスローになる jsk-ros-pkg/jsk_pr2eus#451

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

inverse-kinematicsで手首が360度回転してしまう #87

inverse-kinematicsで手首が360度回転してしまう #87

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

inverse-kinematicsで手首が360度回転してしまう #87

inverse-kinematicsで手首が360度回転してしまう #87

Comments

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014

k-okada commented Mar 3, 2014