search

jsk-ros-pkg / jsk_robot

jsk-ros-pkg/jsk_robot
https://github.com/jsk-ros-pkg/jsk_robot
73 stars 97 forks source link

Baxterの腕がヘッドとコリジョンする #303

Closed aginika closed 9 years ago

aginika commented 9 years ago

ので、以下のようにcollision-avoidance-link-pairを設定したらエラーがでます。 どうするのが適切でしょうか？ headとelbowが当たらないようにしたいです。

(baxter-init)
(objects (list *baxter*))
(send *baxter* :larm :inverse-kinematics (make-coords :pos #f(500 100 500)) :debug-view t :collision-avoidance-link-pair (list (list (elt (send *baxter* :head) 0) (send *baxter* :larm :elbow-p :child-link)))

が

loop:   1
union-link-list: (left_upper_shoulder left_lower_shoulder left_upper_elbow left_lower_elbow left_upper_forearm left_lower_forearm left_wrist)
move-target: (#<cascaded-coords #X8a48568 :larm-end-coords  708.974 522.351 180.297 / -1.129 0.56 -1.936>)
targe-coordst: (#<coordinates #X911cce0  500.0 100.0 500.0 / 0.0 0.0 0.0>)
vel-pos : 13.644 -46.751 -87.339 
vel-rot :  0.360  -0.342   0.061 
vel-pos-norm : 100.000/  1.000
vel-rot-norm :   0.500/  0.017
angle :   20.0   -25.0   -40.0    60.0   -20.0    80.0     0.0 
 min  :  -97.5  -123.0  -175.0    -2.9  -175.3   -90.0  -175.3 
 max  :   97.5    60.0   175.0   150.0   175.3   120.0   175.3 
usrwei:  1.000   1.000   1.000   1.000   1.000   1.000   1.000 
ocost :100000000000000000000.000 100000000000000000000.000 100000000000000000000.000 100000000000000000000.000 100000000000000000000.000 100000000000000000000.000 100000000000000000000.000 
cost  :  0.263   0.090   0.167   0.284   0.077   1.778   0.000 
weight:  1.000   1.000   1.000   1.000   1.000   1.000   1.000 
k     :  0.000 (manipulability:  0.118, gain:  0.001, limit:  0.100, len:7)
;; ERROR : (car pair) is not included in link-list (head left_lower_elbow)
/opt/ros/hydro/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: integer expected in (right_w2_jt . #<rotational-joint #X6a45350 right_w2>)

となってしまいます。

k-okada commented 9 years ago

send robot :links で出てくるリンクをcollision pairには含める必要が有ると思います．

◉ Kei Okada

On Tue, Apr 28, 2015 at 7:13 PM, Yuto Inagaki notifications@github.com wrote:

ので、以下のようにcollision-avoidance-link-pairを設定したらエラーがでます。 どうするのが適切でしょうか？ headとelbowが当たらないようにしたいです。

(baxter-init) (objects (list baxter)) (send baxter :larm :inverse-kinematics (make-coords :pos #f(500 100 500)) :debug-view t :collision-avoidance-link-pair (list (list (elt (send baxter :head) 0) (send baxter :larm :elbow-p :child-link)))

が

loop: 1 union-link-list: (left_upper_shoulder left_lower_shoulder left_upper_elbow left_lower_elbow left_upper_forearm left_lower_forearm left_wrist) move-target: (#<cascaded-coords #X8a48568 :larm-end-coords 708.974 522.351 180.297 / -1.129 0.56 -1.936>) targe-coordst: (#<coordinates #X911cce0 500.0 100.0 500.0 / 0.0 0.0 0.0>) vel-pos : 13.644 -46.751 -87.339 vel-rot : 0.360 -0.342 0.061 vel-pos-norm : 100.000/ 1.000 vel-rot-norm : 0.500/ 0.017 angle : 20.0 -25.0 -40.0 60.0 -20.0 80.0 0.0 min : -97.5 -123.0 -175.0 -2.9 -175.3 -90.0 -175.3 max : 97.5 60.0 175.0 150.0 175.3 120.0 175.3 usrwei: 1.000 1.000 1.000 1.000 1.000 1.000 1.000 ocost :100000000000000000000.000 100000000000000000000.000 100000000000000000000.000 100000000000000000000.000 100000000000000000000.000 100000000000000000000.000 100000000000000000000.000 cost : 0.263 0.090 0.167 0.284 0.077 1.778 0.000 weight: 1.000 1.000 1.000 1.000 1.000 1.000 1.000 k : 0.000 (manipulability: 0.118, gain: 0.001, limit: 0.100, len:7) ;; ERROR : (car pair) is not included in link-list (head left_lower_elbow) /opt/ros/hydro/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: integer expected in (right_w2_jt . #<rotational-joint #X6a45350 right_w2>)

となってしまいます。

— Reply to this email directly or view it on GitHub https://github.com/jsk-ros-pkg/jsk_robot/issues/303.

aginika commented 9 years ago

以下のように、headもleft_lower_elbowもあるように見えるのですが、 書き方がよくないのでしょうか？

$ send *baxter* :links
(#<bodyset-link #X5aaf5f0 base  0.0 0.0 0.0 / 0.0 0.0 0.0> #<bodyset-link #X59b9ba8 head  60.0 0.0 686.0 / 0.0 0.0 0.0> #<bodyset-link #X59c8c40 left_upper_shoulder  64.027 259.027 129.626 / 1.134 0.0 0.0> #<bodyset-link #X59ca410 left_lower_shoulder  93.188 321.563 399.976 / 1.134 -0.436 -1.571> #<bodyset-link #X59bafb0 left_upper_elbow  132.256 405.345 443.083 / 0.03 0.767 -0.943> #<bodyset-link #X59bcab0 left_lower_elbow  282.406 622.397 506.082 / 0.485 0.401 -2.256> #<bodyset-link #X5809cb8 left_upper_forearm  366.781 666.893 465.687 / -1.314 0.49 -2.029> #<bodyset-link #X59bcfd8 left_lower_forearm  589.515 774.638 355.419 / -1.129 0.56 2.777> #<bodyset-link #X59bf858 left_wrist  631.558 685.847 293.786 / -2.499 -0.307 -2.162> #<bodyset-link #X5a363e0 right_upper_shoulder  64.027 -259.027 129.626 / -1.134 0.0 0.0> #<bodyset-link #X5a36518 right_lower_shoulder  93.188 -321.563 399.976 / -1.134 -0.436 -1.571> #<bodyset-link #X5a3cde8 right_upper_elbow  132.256 -405.345 443.083 / -0.03 0.767 0.943> #<bodyset-link #X5a18a70 right_lower_elbow  282.406 -622.397 506.082 / -0.485 0.401 -0.886> #<bodyset-link #X59f6098 right_upper_forearm  366.781 -666.893 465.687 / 1.314 0.49 2.029> #<bodyset-link #X59fb7f8 right_lower_forearm  589.515 -774.638 355.419 / 1.129 0.56 0.365> #<bodyset-link #X59f0910 right_wrist  631.558 -685.847 293.786 / 2.499 -0.307 2.162>)
snozawa commented 9 years ago

collision pairをみせてください． 多分ですが，left_lowe_elbow headの順になってないといけないと思います． collision pairのcarがlink-listに含まれる必要があり，今は:larm :inverse-kinematicsなので 左腕のリンクがはいってる必要があります．

aginika commented 9 years ago

@snozawa ありがとうございます！。まさに順番でした。 collision pairはどう取得すればよいのでしょうか？ とりあえず、inverse-kinematicsでcollision-avoidance-link-pairの 引数では head left_low_elbowになっていたので、これを逆順にすると計算がされたように思われます。

send *baxter* :larm :inverse-kinematics (make-coords :pos #f(500 100 500)) :debug-view t :collision-avoidance-link-pair (list (list  (send *baxter* :larm :elbow-p :child-link) (elt (send *baxter* :head) 0)))
aginika commented 9 years ago

send *baxter* :linksにheadのpanelがないため、図のように ヘッドとelbow, ヘッドとwristがぶつからないikを解けるのですが、panelとの衝突を考慮することができません。 どうすべきなのでしょうか? baxter_collision

send *baxter* :larm :inverse-kinematics (make-coords :pos #f(500 100 500)) :debug-view t
:collision-avoidance-link-pair (list (list (send *baxter* :larm :wrist-p :child-link) (elt (send *baxter* :head) 0)) (list (send *baxter* :larm :elbow-p :child-link) (elt (send *baxter* :head) 0)))
snozawa commented 9 years ago

collision pairはどう取得すればよいのでしょうか？

?collision pairは@agnikaさんが決めて引数で与えてるのではないでしょうか？

ヘッドのパネルの問題は、ヘッドのパネルのlinkもpairにいれてあげれば良いとおもいます。 つまり、pairは l-wristとhead0 l-elbowとhead0 l-wristとhead1 l-elbowとhead1 の４つをいれてみてください。

send baxter :linksにheadのpanelがないため、

これは問題ないと思います。 collision link pairは、各ペアのcarがIKのlink-listに含まれていればOKです。 https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L1281 これは、干渉回避計算のなかでヤコビアンを計算するわけですが、そのときにlink-listに含まれてないと 「このcollisionペアは今のIKに関係なくない？」とおこられます。 https://github.com/jsk-ros-pkg/jsk_robot/issues/303#issue-71545197 の ;; ERROR : (car pair) is not included in link-list (head left_lower_elbow) はそういう意図のワーニングです。

なので、各ペアのcdrはlinksに含まれる必要はないです。 https://github.com/euslisp/jskeus/blob/master/irteus/demo/hanoi-arm.l#L43 のサンプルも参考にしてみてください。 これは、アームロボットが目の前の棒をよけてくれます。 棒はアームロボットのlinksには含まれてません。

ところで、@agnikaさんほどのプロでも今回のPR関係で分かりにくかったところは何でしょうか。 そこはぜひdocumentを追加すべきポイントな気がします。

aginika commented 9 years ago

?collision pairは@agnikaさんが決めて引数で与えてるのではないでしょうか？

なるほど、collision pair は collision avoidance link pairのことなのですね。すいませんでした、渡しています。

これは問題ないと思います。 collision link pairは、各ペアのcarがIKのlink-listに含まれていればOKです。

なるほど、ありがとうございます。勉強になりました。

ところで、@agnikaさんほどのプロでも今回のPR関係で分かりにくかったところは何でしょうか。 そこはぜひdocumentを追加すべきポイントな気がします。

プロではない僕の知識不足のところが大きいのでなんとも言えないですが、 jmanual.pdfやdemoの中のgrepでもcollision-avoidance-link-pairの使い方が見つからなかったことでしょうか？:collision-avoidance, :collision-avoidance-link-pair-from-link-list とかの関数もありますが、:inverse-kinematicsでやることとの違いもわからなかったです。

aginika commented 9 years ago

おかげさまで想定した動きが得られました。ありがとうございました。

 send *baxter* :larm :inverse-kinematics (make-coords :pos #f(500 100 500)) :debug-view t :collision-avoidance-link-pair (list (list (send *baxter* :larm :wrist-p :child-link) (send *baxter* :screen_lk)) (list (send *baxter* :larm :elbow-p :child-link) (send *baxter* :screen_lk)) (list (send *baxter* :larm :elbow-r :child-link) (send *baxter* :screen_lk)))

baxter_collision_check

aginika commented 9 years ago

なんども申し訳ありません。 baxterを使うときにどんなときでもinverse-kinematicsの結果がpanelを回避するようにしたいと考えています。(:inverse-kinatmatics , :larm :inverse-kinamatics , :rarm :inverse-kinematics)

これらのcollision pair

 :collision-avoidance-link-pair (list (list (send *baxter* :larm :wrist-p :child-link) (send *baxter* :screen_lk)) (list (send *baxter* :larm :elbow-p :child-link) (send *baxter* :screen_lk)) (list (send *baxter* :larm :elbow-r :child-link) (send *baxter* :screen_lk)))

の設定を毎回反映させるには、どの関数をoverwriteするのが適切なのでしょうか？

k-okada commented 9 years ago

https://github.com/euslisp/jskeus/blob/master/irteus/irtrobot.l#L77 についかしたらいいとおもいます．

◉ Kei Okada

On Wed, Apr 29, 2015 at 5:44 PM, Yuto Inagaki notifications@github.com wrote:

なんども申し訳ありません。 baxterを使うときにどんなときでもinverse-kinematicsの結果がpanelを回避するようにしたいと考えています。( :inverse-kinatmatics , :larm :inverse-kinamatics , :rarm :inverse-kinematics)

これらのcollision pair

:collision-avoidance-link-pair (list (list (send baxter :larm :wrist-p :child-link) (send baxter :screen_lk)) (list (send baxter :larm :elbow-p :child-link) (send baxter :screen_lk)) (list (send baxter :larm :elbow-r :child-link) (send baxter :screen_lk)))

の設定を毎回反映させるには、どの関数をoverwriteするのが適切なのでしょうか？

— Reply to this email directly or view it on GitHub https://github.com/jsk-ros-pkg/jsk_robot/issues/303#issuecomment-97353897 .

k-okada commented 9 years ago

https://github.com/euslisp/jskeus/blob/master/irteus/demo/sample-robot-model.l#L113 でした．

◉ Kei Okada

2015-04-29 17:50 GMT+09:00 Kei Okada k-okada@jsk.t.u-tokyo.ac.jp:

https://github.com/euslisp/jskeus/blob/master/irteus/irtrobot.l#L77 についかしたらいいとおもいます．

◉ Kei Okada

On Wed, Apr 29, 2015 at 5:44 PM, Yuto Inagaki notifications@github.com wrote:

なんども申し訳ありません。 baxterを使うときにどんなときでもinverse-kinematicsの結果がpanelを回避するようにしたいと考えています。( :inverse-kinatmatics , :larm :inverse-kinamatics , :rarm :inverse-kinematics)

これらのcollision pair

:collision-avoidance-link-pair (list (list (send baxter :larm :wrist-p :child-link) (send baxter :screen_lk)) (list (send baxter :larm :elbow-p :child-link) (send baxter :screen_lk)) (list (send baxter :larm :elbow-r :child-link) (send baxter :screen_lk)))

の設定を毎回反映させるには、どの関数をoverwriteするのが適切なのでしょうか？

— Reply to this email directly or view it on GitHub https://github.com/jsk-ros-pkg/jsk_robot/issues/303#issuecomment-97353897 .

aginika commented 9 years ago

ありがとうございます。 リンクを参考に以下のような一文をbaxter-interfaceの:initに書きましたが、

   (setq collision-avoidance-links (list (send self :robot :screen_lk) (send self :robot :larm :wrist-p :child-link) (send self :robot :larm :elbow-p :child-link) (send self :robot :larm :elbow-r :child-link) (send self :robot :rarm :wrist-p :child-link) (send self :robot :rarm :elbow-p :child-link) (send self :robot :rarm :elbow-r :child-link)))

以下のようにやってもdebug-viewには反映されず、実際にめり込んだ姿勢を返してきます。

(send *baxter* :larm :inverse-kinematics (make-coords :pos #f(500 100 500)) :debug-view t)

どのようにするのがよいでしょうか？

k-okada commented 9 years ago

robotのモデルのインスタンスを変えないとダメでないかい．

◉ Kei Okada

On Wed, Apr 29, 2015 at 7:22 PM, Yuto Inagaki notifications@github.com wrote:

ありがとうございます。 リンクを参考に以下のような一文をbaxter-interfaceの:initに書きましたが、

(setq collision-avoidance-links (list (send self :robot :screen_lk) (send self :robot :larm :wrist-p :child-link) (send self :robot :larm :elbow-p :child-link) (send self :robot :larm :elbow-r :child-link) (send self :robot :rarm :wrist-p :child-link) (send self :robot :rarm :elbow-p :child-link) (send self :robot :rarm :elbow-r :child-link)))

以下のようにやってもdebug-viewには反映されず、実際にめり込んだ姿勢を返してきます。

(send baxter :larm :inverse-kinematics (make-coords :pos #f(500 100 500)) :debug-view t)

どのようにするのがよいでしょうか？

— Reply to this email directly or view it on GitHub https://github.com/jsk-ros-pkg/jsk_robot/issues/303#issuecomment-97379318 .

aginika commented 9 years ago

あ、・・・ ありがとうございます。 出直します。

aginika commented 9 years ago

以下のような継承クラスを作ったのですが、 まだだめです・・・

(defclass baxter-robot-safe
  :super baxter-robot
  :slots ())
(defmethod baxter-robot-safe
  (:init (&rest args)
         (send-super* :init args)
         (setq collision-avoidance-links (list (send self :screen_lk)
                                               (send self :larm :wrist-p :child-link)
                                               (send self :larm :elbow-p :child-link)
                                               (send self :larm :elbow-r :child-link)
                                               (send self :rarm :wrist-p :child-link)
                                               (send self :rarm :elbow-p :child-link)
                                               (send self :rarm :elbow-r :child-link)))
         )
  )

(defun baxter-init-safe ()
  (if (not (boundp '*ri*))
      (setq *ri* (instance baxter-interface :init)))
  (if (not (boundp '*baxter*))
      (setq *baxter* (instance baxter-robot-safe :init)))
  (send *ri* :calib-grasp :arms)
  )
k-okada commented 9 years ago

これできたんだっけ？いずれにせよ，できたら， https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L1209 に説明をいれておいてください． https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L1087 なかんじ．これがあれば，自動で， http://euslisp.github.io/jskeus/ と http://euslisp-docs.readthedocs.org/en/latest/jskeus/jmanual/ にでてきます．

◉ Kei Okada

On Wed, Apr 29, 2015 at 10:36 PM, Yuto Inagaki notifications@github.com wrote:

以下のような継承クラスを作ったのですが、 まだだめです・・・

(defclass baxter-robot-safe :super baxter-robot :slots ()) (defmethod baxter-robot-safe (:init (&rest args) (send-super* :init args) (setq collision-avoidance-links (list (send self :screen_lk) (send self :larm :wrist-p :child-link) (send self :larm :elbow-p :child-link) (send self :larm :elbow-r :child-link) (send self :rarm :wrist-p :child-link) (send self :rarm :elbow-p :child-link) (send self :rarm :elbow-r :child-link))) ) )

(defun baxter-init-safe () (if (not (boundp 'ri)) (setq ri (instance baxter-interface :init))) (if (not (boundp 'baxter)) (setq baxter (instance baxter-robot-safe :init))) (send ri :calib-grasp :arms) )

— Reply to this email directly or view it on GitHub https://github.com/jsk-ros-pkg/jsk_robot/issues/303#issuecomment-97428411 .

k-okada commented 9 years ago

以下が正解みたいです．

(defclass baxter-robot-safe
  :super baxter-robot
  :slots ())
(defmethod baxter-robot-safe
  (:init (&rest args)
         (send-super* :init args)
         (setq collision-avoidance-links (list (send self :screen_lk)
                                               (send self :larm :wrist-p :child-link)
                                               (send self :larm :elbow-p :child-link)
                                               (send self :larm :elbow-r :child-link)
                                               (send self :rarm :wrist-p :child-link)
                                               (send self :rarm :elbow-p :child-link)
                                               (send self :rarm :elbow-r :child-link)))
         (setq larm-collision-avoidance-links collision-avoidance-links)
         (setq rarm-collision-avoidance-links collision-avoidance-links)
         )
  )

https://github.com/euslisp/jskeus/blob/master/irteus/irtrobot.l#L132

の

(send self limb :collision-avoidance-links)

でかかっていました．

なんでこうなっているんだろう？

最近はfullbodyメインだからあまりつかわれていないのかな． limb :collision-avoidance-links がnilならcollision-avoidance-linksをみるようにする，とかしたら， 副作用大きい＆複雑すぎるかな．

snozawa commented 9 years ago

このあたりの部分、2010年くらいのできたてほやほやの時期に僕が追加したらしいです。 おおもとのコードは、(rbrainをみると)ar3model.lのときのものでした。 なんでこうしてたかかちょっと思い出せませんが、

limb :collision-avoidance-links がnilならcollision-avoidance-linksをみるようにする，とかしたら， 副作用大きい＆複雑すぎるかな．

これに加えて、rarm-collision-avoidance-links、larm-collision-avoidance-linksをそもそも なくしても良いような気がしてきました。 （どのIKも、常にcollision-avoidance-linksから必要なものをとってくる）

aginika commented 9 years ago

@k-okada @snozawa ありがとうございます。 baxter-interface.lに加えてしまおうと思います。

k-okada commented 9 years ago

なんとなくだけど，昔は send robot :larm :collision-avoidance-links みたいにlimb毎にセットしていた（ではないかと予想）ものの名残かもしれないですね．

◉ Kei Okada

2015-05-02 0:55 GMT+09:00 Shunichi Nozawa notifications@github.com:

このあたりの部分、2010年くらいのできたてほやほやの時期に僕が追加したらしいです。 おおもとのコードは、(rbrainをみると)ar3model.lのときのものでした。 なんでこうしてたかかちょっと思い出せませんが、

limb :collision-avoidance-links がnilならcollision-avoidance-linksをみるようにする，とかしたら， 副作用大きい＆複雑すぎるかな．

これに加えて、rarm-collision-avoidance-links、larm-collision-avoidance-linksをそもそも なくしても良いような気がしてきました。 （どのIKも、常にcollision-avoidance-linksから必要なものをとってくる）

— Reply to this email directly or view it on GitHub https://github.com/jsk-ros-pkg/jsk_robot/issues/303#issuecomment-98164130 .