Refactor to re-use code #1
Conversation
…nterface.l as panda-common-interface.l
|
|
Fixed. |
|
@pazeshun Thank you very much for your effort. How about changing the name of the common interface from |
すみません、ご質問のニュアンスを正しく理解できているか自信がないので、日本語で返事させていただきます。 (case arm
(:rarm (setq acts (list r-gripper-grasp-action)))
(:larm (setq acts (list l-gripper-grasp-action))))みたいに書かないといけなくて、まどろっこしい印象でした。 (gethash arm gripper-grasp-actions)という短い構文で
FR3が手元にないので若干不安ではあるのですが、確かにこうした方がいいかもしれないです。 |
| @@ -24,7 +24,11 @@ | |||
| (cons :controller-action "/position_joint_trajectory_controller/follow_joint_trajectory") | |||
| (cons :controller-state "/position_joint_trajectory_controller/state") | |||
| (cons :action-type control_msgs::FollowJointTrajectoryAction) | |||
| (cons :joint-names (send-all (send robot :joint-list) :name)))))) | |||
| (cons :joint-names (send-all (send robot :joint-list) :name))))) | |||
| (:gripper-grasp-action | |||
There was a problem hiding this comment.
@yuki-asano このmethod、本当に必要でしょうか。
そもそも、ユーザーがgripper-grasp-actionsにダイレクトにアクセスすることは想定してなくて、全て:start-grasp経由で動かす、というのが想定していた使い方です。
gripper-grasp-actionsにアクセスしたい用途がある場合、その用途はpandaユーザー共通で使えるものである可能性が高く、panda-common-interface.lに入れ込む方向で実装したいので、詳細をお教えいただけますと幸いです。
仮に、その用途がデモspecificすぎてpanda-common-interface.lに入らない場合であっても、ユーザーの手元のコードで、*ri*を作る前に、
(defmethod panda-robot-interface
(:new-method ()
(send (gethash :rarm gripper-grasp-actions) ...)
)みたいに書いて新しいmethodを定義するのが正攻法な気がしました。
例: https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/0.3.14/pr2eus/test/pr2-ri-test.l#L7-L24
There was a problem hiding this comment.
hashについては，承知しました．経緯の説明ありがとうございます．
panda.launchもfranka.launchにしちゃってもいいことになりますか？
これは，panda.launchのままで，fr3.launchも新しく作るので良いのではないかと思います．
他のところもpandaのままでいいのではないかと思います．
gripper-grasp-actionsにアクセスしたい用途がある場合、その用途はpandaユーザー共通で使えるものである可能性が高く、panda-common-interface.lに入れ込む方向で実装したいので、詳細をお教えいただけますと幸いです。
以下のような形で，euslispコードの中でgrasp actionのresultを取得できるようにしておきたいと思っているのですが，他にやり方があるならそれでも大丈夫なのでやり方を教えてもらえると助かります．
(setq isGraspSuccess (send (send gripper-grasp-action :get-result) :success))
みたいに書いて新しいmethodを定義するのが正攻法な気がしました。
微妙だったらこのやり方でも大丈夫です．
There was a problem hiding this comment.
これは，panda.launchのままで，fr3.launchも新しく作るので良いのではないかと思います．
他のところもpandaのままでいいのではないかと思います．
こちらですが、もう一度よく考えたところ、JSKとしてはロボットのlaunchを上げた時にposition_joint_trajectory_controllerも起動してほしい、ということで、franka_control.launch+position_joint_trajectory_controller_spawnerのfranka.launchを作って、それをpanda.launchでincludeすべきであるという結論に至ったので、そのように修正してみました。
以下のような形で，euslispコードの中でgrasp actionのresultを取得できるようにしておきたいと思っているのですが，他にやり方があるならそれでも大丈夫なのでやり方を教えてもらえると助かります．
すみません、この需要があることをすっかり失念していました。
(send (send *ri* :get-start-grasp-result :rarm) :success)のようにアクセスできるようにしてみました。
似たようなmethodがPR2にあって、それと大体同じことをしています。
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/0.3.15/pr2eus/pr2-interface.l#L323-L348
(:wait-for-resultするかしないかの場合分けがありますが、常に:wait-for-result呼ぶので問題ないはずなのでそうしてあるのと、返り値は:successだけ抜き出さずにmessageのインスタンスをそのまま返すことで、:errorも上位レイヤから見れるようにしてあります。)
There was a problem hiding this comment.
gripper-grasp-actions
には、hashが入っているので、actionにアクセスするには、
(gethash :rarm gripper-grasp-actions)
としないといけなさそうです。
0066eb7
のようにすると、ひとまずは大丈夫になりそうです。
また、:start-graspしたときにwaitが送られていないような気がしており、
waitの引数に矛盾があるような気がするのですがどうでしょうか。
リンクがうまく貼れないのですが以下のあたりです。
204行目
(:send-gripper-grasp-action
(arm width speed force &key (wait t) (inner 0.005) (outer 0.07))
349行目
(:gripper-method-with-width-helper
(action-sender actions arm wait width tm &rest args)
376行目
(send self :gripper-method-with-width-helper
:send-gripper-grasp-action gripper-grasp-actions arm wait width tm
effort :inner inner :outer outer)
There was a problem hiding this comment.
0066eb7
のようにすると:start-graspや:get-start-grasp-resultでエラーが出る返り値がnilになる（さらに:start-graspでは:wait tを与えてもwaitできなくなる）気がするのですが、このcommitを入れる前の状態でどういうコードを実行して、どのようなエラーが出たのですか？
gripper-grasp-actions
には、hashが入っているので、actionにアクセスするには、
(gethash :rarm gripper-grasp-actions)
としないといけなさそうです。
(gethash :rarm gripper-grasp-actions)に相当する操作は、:send-gripper-action-methodの中でやっているので、
https://github.com/pazeshun/jsk_robot/blob/499d90dd18a41b3fa6ffc0e31cd31f513569a97f/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l#L262
:gripper-action-postprocessで:send-gripper-action-methodに渡すactionsはhash-tableで
あることを想定しており、:gripper-action-postprocess内でhash-tableじゃないものに変換されるとエラーになる正しく動作しなくなるはずです。
僕の書いたコードでは、actionsはhash-table、actionはactionのinstanceになるようになってるはずです。
（なお、hashというのはハッシュ値のことであって、これを使ったデータ構造がhash-tableなので、この二つの用語の区別を意識された方がよろしいかと思います。）
また、
:start-graspしたときにwaitが送られていないような気がしており、
waitの引数に矛盾があるような気がするのですがどうでしょうか。
「送られていない」のではなく、:start-graspのwaitのデフォルト値がnilになっていて、:send-gripper-grasp-actionのwaitのデフォルト値がtであることと矛盾している、ということで合ってますでしょうか。
これは、僕がpanda使い始める前からそうなっていて、backward compatibilityのためにそのままにしている、ということです。
https://github.com/ykawamura96/jsk_robot/blob/add_panda_robot/jsk_panda_robot/panda_eus/euslisp/dual_panda-interface.l#L332
https://github.com/ykawamura96/jsk_robot/blob/add_panda_robot/jsk_panda_robot/panda_eus/euslisp/dual_panda-interface.l#L263
@yuki-asano のコードでもそうなっているかと思います。
https://github.com/jsk-ros-pkg/jsk_robot/pull/1799/files#diff-49407bb0669ccb3405b9c922a92547506aea3393e03e7c9aa2615efe645ed989R173
https://github.com/jsk-ros-pkg/jsk_robot/pull/1799/files#diff-49407bb0669ccb3405b9c922a92547506aea3393e03e7c9aa2615efe645ed989R109
なお、今確認してみると、euslispの標準では:start-graspのwaitのデフォルト値はtですね。
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/8fc634d0d92830c201f9ae922c5e673c0def0b19/pr2eus/pr2-interface.l#L264
https://github.com/jsk-ros-pkg/jsk_robot/blob/a4b99e4a54afde3df80159435a7db4b90ea45627/jsk_fetch_robot/fetcheus/fetch-interface.l#L209
tに変更すべきかもしれないですが、僕以前にpandaのeuslispを使ったデモが散逸していて確認が大変で、なるべくAPIをそのままにしようとした結果そのままになった、という経緯で、正直あまり変更したくはないです。
There was a problem hiding this comment.
確認ありがとうございます。
以下のようなエラーだったのですが、
1.irteusgl$ (send (send *ri* :get-start-grasp-result :rarm) :success)
[ERROR] [1685594397.753742185]: [/franka_gripper/grasp] :wait-for-result (return nil when no goal exists)
Call Stack (max depth: 20):
0: at (send (send ros::comm-state :latest-result) :result)
1: at (apply #'send action method args)
2: at (apply #'send action method args)
3: at (send* action method args)
4: at (if action (send* action method args) nil)
5: at (let ((action (gethash (send self :expand-arm-alias arm) actions))) (if action (send* action method args) nil))
6: at (send self :send-gripper-action-method arm actions :get-result)
7: at (if t (send self :send-gripper-action-method arm actions :get-result) nil nil)
8: at (if (eq #:case4453 ':arms) (mapcar #'(lambda (a) (send self :send-gripper-action-method a actions :get-result)) arms) (if t (send self :sen\
d-gripper-action-method arm actions :get-result) nil nil) nil)
9: at (let ((#:case4453 arm)) (if (eq #:case4453 ':arms) (mapcar #'(lambda (a) (send self :send-gripper-action-method a actions :get-result)) arm\
s) (if t (send self :send-gripper-action-method arm actions :get-result) nil nil) nil))
10: at (case arm (:arms (mapcar #'(lambda (a) (send self :send-gripper-action-method a actions :get-result)) arms)) (t (send self :send-gripper-a\
ction-method arm actions :get-result)))
11: at (let ((arms (send self :arm2arms arm))) (if wait (dolist (a arms) (send self :send-gripper-action-method a actions :wait-for-result))) (ca\
se arm (:arms (mapcar #'(lambda (a) (send self :send-gripper-action-method a actions :get-result)) arms)) (t (send self :send-gripper-action-method\
arm actions :get-result))))
12: at (send self :gripper-action-postprocess arm gripper-grasp-actions t)
13: at (send *ri* :get-start-grasp-result :rarm)
14: at (send (send *ri* :get-start-grasp-result :rarm) :success)
15: at #<compiled-code #X560fc8d84e18>
/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :result in (send (send ros::comm-state :latest-result) :\
result)
この前に一回start-graspを実行すると、エラーにならないようでした。失礼しました。
2.E1-irteusgl$ send *ri* :start-grasp :rarm
#<franka_gripper::graspresult #X560fde607f50>
3.E1-irteusgl$ (send (send *ri* :get-start-grasp-result :rarm) :success)
t
waitについては
0066eb7
の副作用になってしまっているようで、こちらも失礼しました。
revertしました。
fc875a0
…stop-grasp/:move-gripper from their return values
…stop-grasp/:move-gripper from separated methods
This reverts commit 0066eb7.
|
@pazeshun 自分の理解不足でまどろっこしいところもあったかと思いますが，色々対応してくれてありがとうございます． |
|
@yuki-asano こちらこそご面倒をおかけしすみません。エラーがわかりにくい問題について、#2 で対処したつもりなのでご確認いただけますでしょうか。 |
euscollada is now released on noetic
panda.urdf.xacrobecause it is unnecessary, I think:rarm-controllerand:hand-controllerbecause they didn't work:check-errorwork. Your:check-erroralways returnsnildual_panda-interface.l. I hope you can easily deal with these changes, but please tell me if you cannot:start-grasp:move-gripper:send-gripper-grasp-action:send-gripper-homing-action:send-gripper-move-action:send-gripper-stop-action