[eustf.l] fix bug in ros::create-quaternion-from-rpy

[Naoki-Hiraoka]

@mqcmd196 @tongtybj

ros::create-quaternion-from-rpyが，[w x y z]を返すことが想定されているにもかかわらず，実際には[x y z w]を返していたため，ros::create-quaternion-msg-from-rpyが正しく機能しないというバグがありました．

ros::create-quaternion-from-rpyが，[w x y z]を返すようにしました．

https://www.kazetest.com/vcmemo/quaternion/quaternion.htm

[Naoki-Hiraoka]

ros::create-quaternion-from-rpy は、返り値の順番が正しくないだけでなく、zの値の計算も正しくないことが分かりました。

• テストコードを作りました。このPRが無い場合の結果はhttps://github.com/jsk-ros-pkg/jsk_roseus/pull/663 にあります。
• jsk-ros-pkg, start-jsk でros::create-quaternion-from-rpyを使っているコードはありませんでした。

[Naoki-Hiraoka]

@mqcmd196 ros::create-quaternion-from-rpy は、返り値の順番が正しくないだけでなく、zの値の計算も正しくなかったので、使う場合は https://github.com/jsk-ros-pkg/jsk_roseus/pull/662/commits/213cc878c38f6aaf3959363dfeda1385eeaf9d67 の変更内容も反映させてください。

[tongtybj]

最近実装ですごく気になったのが、Euslispの関数rpy-angleの返り値は((yaw1 pitch1 roll1) (yaw2 ptich2 roll2)) になっていますね。 Jmanual.pdfの Sec. 13.2にも書いてあるように:

rpy-angle 関数は、回転行列をワールド座標系における z,y,x 軸回りの 3 つの回転角に分解する。

命名規則が少しきになるところです (ypr-angleにしたほうがしっくりくるのでは？)

[mqcmd196]

@Naoki-Hiraoka ありがとうございます．リリース前はこちらの変更を適用しておきます．

[knorth55]

命名規則が少しきになるところです (ypr-angleにしたほうがしっくりくるのでは？)

@tongtybj Euslispはrpyといいながら知っている限り、ほとんどyprで定義されていると思います。 命名かえるのは多くの関数名をかえることになるかとおもいます。

[pazeshun]

@tongtybj @knorth55

命名規則が少しきになるところです (ypr-angleにしたほうがしっくりくるのでは？)

コメント当時に気付けなくて申し訳なかったのですが、rpy-angleで正しいと思っています。 というのは、rpy-angleとは「ロール・ピッチ・ヨー角」を意味していて、これはZYXオイラー角を意味しているからです。 http://www.mech.tohoku-gakuin.ac.jp/rde/contents/course/robotics/coordtrans_s.html https://www.sports-sensing.com/brands/labss/motionmeasurement/motion_biomechanics/euler_angle_expression.html つまり、ローカル座標系のZ軸回り→Y軸回り→X軸回りの順で回していくことを意味しているので、その順で並んだ角度が返ってくるのは不自然ではないと思っています。 「ロール・ピッチ・ヨー角」という用語自体が直感的でなくて問題、ということになるでしょうか（これを嫌ってか、「ヨー・ピッチ・ロール」と並べられることもあります）。