rad表記のfloat-vectorをdegに変換する方法について

[softyanija]

タイトル通りradで書かれたfloat-vectorをまるごとdegに変換したいのですが、一発でそのような変換を行ってくれる関数が調べても見つかりません。山口さん(@708yamaguchi)に質問したところ以下のようにするといいと教えていただきました。

(setq joint-states (coerce (mapcar #'(lambda (x) (rad2deg x)) (coerce *\
joints* cons)) float-vector))   

ここではrad表記のjointsをdeg表記の joint-states に変換しています。
他に良い方法が合ったら教えていただけると幸いです。

[Naoki-Hiraoka]

上の例と大差ありませんが，
(map float-vector #'rad2deg #F(1 2 3))
でできます．

[k-okada]

ちなみに，これ，どこで必要なのかな． eusの世界でプログラムを書く時点では degで扱いたいので， ROSでradで出てきた情報は，*ri* から取り出す時点で，deg になっているのが望ましいです． https://github.com/jsk-ros-pkg/jsk_robot/blob/25ac99ab82539b0e95d20470db1675b5f2f58c56/jsk_naoqi_robot/naoqieus/naoqi-interface.l#L125

…

-- ◉ Kei Okada 2021年6月18日(金) 16:07 Naoki Hiraoka ***@***.***>:

上の例と大差ありませんが， (map float-vector #'rad2deg #F(1 2 3)) でできます． — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub <#674 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AADYNXBPJ6UI4XNICHGIR3DTTLWBDANCNFSM465CREXA> .

[softyanija]

@Naoki-Hiraoka
短くて楽ですね。ありがとうございます。

@k-okada
83C1のdual_pandaだと/dual_panda/joint_statesという形でjointの情報が出ていて、そこから :position を取ってくるとrad表記になっていました。
実機に繋いで (send rI :potentio-vector) のようにするとdegで出てきますが、rosbagに対しての使用のためにこのようにしています。

[k-okada]

ROSのクライアントからみたら，データを取得するうえでは実機でもrosbagでも同じなのでrosbag からでも *ri* :potentio-vector できるはずです．わからなかったらrosbag 送って下さい． -- ◉ Kei Okada 2021年6月18日(金) 16:41 Koki Amabe ***@***.***>:

…

@Naoki-Hiraoka <https://github.com/Naoki-Hiraoka> 短くて楽ですね。ありがとうございます。 @k-okada <https://github.com/k-okada> 83C1のdual_pandaだと/dual_panda/joint_statesという形でjointの情報が出ていて、そこから :position を取ってくるとrad表記になっていました。 実機に繋いで (send *rI* :potentio-vector) のようにするとdegで出てきますが、rosbagに対しての使用のためにこのようにしています。 — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#674 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AADYNXASCMTPQAGFZXCSQZDTTL2DXANCNFSM465CREXA> .

[softyanija]

https://github.com/softyanija/jsk_robot/blob/add_panda_robot/jsk_panda_robot/panda_eus/euslisp/end-coords_publisher.l
このプログラムでの話で、(dual_panda-init) で ri を作ろうとすると下のメッセージのようにactoin serverが見つからないと言われます

74.irteusgl$ (dual_panda-init)                                              
[ WARN] [1624007550.969777206, 1623906081.179971108]: topic /robot_interfac\
e_marker_array already advertised                                           
[ WARN] [1624007550.973012407, 1623906081.179971108]: topic /dual_panda/dua\
l_panda_effort_joint_trajectory_controller/follow_joint_trajectory/goal alr\
eady advertised                                                             
[ WARN] [1624007550.973038241, 1623906081.179971108]: topic /dual_panda/dua\
l_panda_effort_joint_trajectory_controller/follow_joint_trajectory/cancel a\
lready advertised                                                           
[ WARN] [1624007550.976105153, 1623906081.189986647]: Waiting for actionlib\
 interface forever because controller-timeout is nil                        
[ WARN] [1624007556.078800648, 1623906086.295045299]: Still waiting for [/d\
ual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajec\
tory] action server. 5 seconds have passed.

rosbagはメールで送ります。 ドライブに上げておきます。このブランチの状態で動かしてます。https://github.com/softyanija/jsk_robot/tree/add_panda_robot

[softyanija]

https://drive.google.com/file/d/1jlGVaAFMPu-qu8_sADgBGZIGqDpN2J2Q/view?usp=sharing にrosbagを置いておきました

[k-okada]

ykawamura96/jsk_robot@b6c87f2 をして

(load "package://panda_eus/euslisp/dual_panda-interface.l")

(defmethod dual_panda-robot-interface
  (:dummy-controller (&rest args)))
(setq *ri* (instance dual_panda-robot-interface :init
                     :type :dummy-controller))

(do-until-key
 (print (send *ri* :state :potentio-vector))

でbagfile の情報が　(send *ri* :state ) に反映されるとおもいます．

ただ，ソモソモは，手先の位置をグラフにしたいんだよね？

でシンプルにplotjuggler などを使って，tfを見ても，親座標との間の変換にしかならないのが問題だとして，
https://answers.ros.org/question/252572/how-to-plot-positionx-y-z-and-velocity-of-fixed-framesend-effector-in-rqt/
みると，https://jsk-visualization.readthedocs.io/en/latest/jsk_rviz_plugins/plugins/tf_trajectory.html が紹介されている．．．

作戦は２つあって，
１）ロボット座標系ー手先座標系の変換をPose等で出力してプロットする方法．

これは，https://github.com/jsk-ros-pkg/jsk_common/blob/master/doc/jsk_topic_tools/scripts/tf_to_pose.rst
等で行うことができる．

２）ロボット座標系ー手先座標系の変換のTFを出力してプロットする方法．

これは，https://answers.ros.org/question/193512/rebroadcast-tf-transforms-with-different-frame_id-on-the-fly/ 等がつかえるけど，リリースされているプログラムはなさそう．jsk_topic_tools に入れてもいいけど，どちらかというとplotツール側で座標変換してくれる方が汎用的か．

トリッキーな方法だけど，tfがgeometry_msgs/TransformStamped のリストであることを利用し，ロボット座標系ー手先座標系の変換をTansfromStamped に一回変換して，tfを出力することができる．

以上のプログラムをまとめたものを以下に置きます．
https://gist.github.com/k-okada/8d4f6169216fd168a4649a3c1d12aa22

プログラムを書くときは，誰かのプログラムを再利用できないか，あるいは，このプログラムはどこまでの範囲で使ってもらえそうか，というのを考えるのが良いです．

実利的には，例えば，https://github.com/softyanija/jsk_robot/blob/dc635d412cb2d42fcb0595ed120ec5a92599b434/jsk_panda_robot/panda_eus/euslisp/end-coords_publisher.l#L41 はロボットが計測した時刻の関節情報を使ってPoseStampedを作っていますが，そのヘッダのタイムスタンスはインスタンスが作られた時刻になっているので，微妙に正確でない情報をプロットすることになる可能性が高いです．

理念としては，汎用的．抽象的な考え方をする良い機会になると思います．

c.f. https://answers.ros.org/question/58793/how-to-use-rxplot-to-plot-data-on-tf/#380659

[softyanija]

画像などの情報まで使うようになると時間的に微妙に正確でないことが効いてきそうなのでそれまでに改良したいですね。他のタスクが多く手が回りきらないので後ほど試して報告します。

[YoheiKakiuchi]

問題の本質とはずれてる気もしますが、
以下だと実行速度、メモリ使用(GC回避)ともに良いと思います。
(joint-statesはfloat-vectorと仮定していて、内容が変更されます)

(scale (rad2deg 1.0) joint-states joint-states)

@softyanija @Naoki-Hiraoka
の結果は正しいですし、意味的な読みやすさはそちらの方が良いかと思います。
僕のはeus初期のCPUが貧弱だったころのやり方ですね。