add condition for wheter there is virtual-link or not

[terasawa]

virtual-linkが存在してもparent-linkを参照しにいってエラーが出ないように、parent-linkが存在するかどうかの条件式にvirtual-linkに関する条件を加えました。

[snozawa]

PRお疲れ様です

if文は、

   (when (and parent-link (derivedp (send self :parent) bodyset-link))

とかみたいにするとどうかな。
parent-linkの存在をチェックしたいけど、virtual-linkはたまたまこういう名前をつけてるというものなので、
もう少し一般的なルールとif文のチェックになるといいかと思います

そもそも今回のエラーの原因をおさらいしてみると、

• euslispでは、基本はルートリンクにparent-linkがいないはず
• 逆動力学計算も、ルートリンクのparent-linkはいないはずなので、伝播しないくてよい
• IKなどでvirtual jointをつけたときは、ルートリンクにparent-linkができてしまう。依然として、このときも逆動力学ではルートリンクのparent-linkはたどらなくてよい

ですね。
なので、「ルートリンクである」というのを適切にif文でチェックする必要があります。
@terasawaくんのif文もその方法の１つだと思います。

上記の新しいif文チェックは、parent-linkでなくてparentをみています。
正しく作られてるロボットは、現状euslispでは

• ルートリンク以外のリンクは、parentにリンクくらす（bodyset-link）がきて、parent-linkにも同じものがはいる
• ルートリンクは、parent-linkがなく、parentはロボット自身（jaxon_red-robotとか）になる
• IK時にvirtual joint、virtual linkをルートリンクにくっつけると、ルートリンクのparent-linkはvirtual link、ルートリンクのparentはロボット自身、ロボットのparentにvirtual linkがくる。つまり、parentがロボット自身、というのはかわらない

になります。
なので、parentがbodyset-linkでない、というのがルートリンクの特徴になるので、
これをif文ではじくといいのではと思いました。

上記のif文で動くか試してみてください。

[snozawa]

さらに追加でコメント２点

• コメント追加
  if文のところ２箇所に、root-linkを除外するためですよ、的なコメントをかいておいてください。
• テストコードの追加
  テストコードを追加しましょう。irteus/test/test-irt-motion.lの後ろの方とかでいいかな。
  *sample-robot*というインスタンスが使えると思います。
  テストするといい内容は、with-append-root-jointで囲った:torque-vectorがエラらない、普通によんだ:torque-vectorと同じ値をかえしてくれる

[snozawa]

テストの:torque-vectorは:calc-torqueにしておいてみてください。
また、

     (assert (eq torque-without-root-joint torque-with-root-joint)) 

は

     (assert (eps-v= torque-without-root-joint torque-with-root-joint)) 

かな。