with-append-root-jointにおいて仮想jointのchild-linkがrobotを指している

ロボット全身の運動方程式を求めるため、floating-baseを含めたヤコビアンを求めようとしているのですが、

(with-append-root-joint
 (link-list-with-robot-6dof *robot* (list (cdr (send *robot* :links)))
                            :joint-class 6dof-joint)
 (send *robot* :calc-jacobian-from-link-list
       (mapcar #'(lambda (l) (send *robot* :link-list (send *robot* l :end-coords :parent))) '(:rleg :lleg))
       :move-target (list (send *robot* :rleg-end-coords) (send *robot* :lleg-end-coords))
       :translation-axis (list t t) :rotation-axis (list t t)))

で求めると、floating-baseの回転方向に関する成分が、手計算と一部異なります。

調べてみると、 https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L3014 で仮想ジョイントの:child-linkがロボットを指しているようで、計算がおかしくなっているようです。 :child-linkにロボットのルートリンク(rlink)を設定すれば恐らく解決するのですが、不都合等ありますでしょうか

何か

手計算の値 vs calc-jatobianから得られる値

を比較するテストコードみたいなのってかけるのかな？また、どのようなかんじでずれていますか？

このへんは大丈夫なはずなのですが、ホントにそうかと言われると自身がないです。今euscolladaで変換したロボットを使ってますか？

(send *robot* :worldcoords)
(send (car (send *robot* :links)) :worldcoords)

の２つの値がちがう、というのが根本原因のように思います。

with-append-root-joint関数のこの部分は元のコードがあっていると思うので、もしちがってるとすればヤコビアン計算のほうを直す必要があるかもしれないです。具体的には、かそうジョイントのchild-linkに何がついてるかが問題で、 jointの:joint-angleメソッドで値が更新されたら、child-linkについているインスタンスが translateやrotateで動かされます。かそうジョイントの目的はロボット全体を動かすというものなので、ロボットがchild-linkで正しいです。逆にルートリンクがchild-linkだと、ロボットのインスタンスと、それにassocされてるルートリンクの関係がかわってしまい、関係がおかしくなるきがします。

何か手計算の値 vs calc-jatobianから得られる値を比較するテストコードみたいなのってかけるのかな？また、どのようなかんじでずれていますか？

6dofの部分だけ気合で求めたものが以下のもので https://gist.github.com/hikun5296/e611e5215c6c6d7e5b90e6828c62acdb 比較をすると、

3.irteusgl$ load "fwd-dyn.l"
t
4.irteusgl$ load "package://hrpsys_ros_bridge_tutorials/models/chidori.l"
t
5.irteusgl$ setq *robot* (instance chidori-robot :init)

10.irteusgl$ (setq *m1* (send *robot* :calc-jacobian-from-link-list-with-6dof
(mapcar #'(lambda (l) (send *robot* :link-list (send *robot* l :end-coords :parent))) '(:rleg :lleg))
:move-target (list (send *robot* :rleg :end-coords) (send *robot* :lleg :end-coords))
:translation-axis (list t t) :rotation-axis (list t t)))

11.irteusgl$ (setq *m2* (with-append-root-joint
(link-list-with-robot-6dof *robot* (list (cdr (send *robot* :links)))
:joint-class 6dof-joint)
(send *robot* :calc-jacobian-from-link-list
(mapcar #'(lambda (l) (send *robot* :link-list (send *robot* l :end-coords :parent))) '(:rleg :lleg))
:move-target (list (send *robot* :rleg-end-coords) (send *robot* :lleg-end-coords))
:translation-axis (list t t) :rotation-axis (list t t))))

12.irteusgl$ format-array (m- *m1* *m2*)
         0.000   0.000   0.000   0.000  -1.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   1.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000  -0.000  -1.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   1.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000

13.irteusgl$ format-array *m1*
         1.000   0.000   0.000   0.000  -0.933   0.100  -0.000  -0.000  -0.808  -0.479  -0.150  -0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   1.000   0.000   0.933   0.000  -0.002  -0.002   0.808   0.000   0.000   0.000   0.110   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   1.000  -0.100   0.002   0.000   0.000   0.000  -0.000   0.120   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   1.000   0.000   0.000   0.000   1.000   0.000   0.000   0.000   1.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   0.000   1.000   0.000   0.000   0.000   1.000   1.000   1.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   0.000   0.000   1.000   1.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         1.000   0.000   0.000  -0.000  -0.933  -0.100   0.000   0.000   0.000   0.000   0.000   0.000  -0.000  -0.000  -0.808  -0.479  -0.150  -0.000
         0.000   1.000   0.000   0.933   0.000  -0.002   0.000   0.000   0.000   0.000   0.000   0.000  -0.002   0.808   0.000   0.000   0.000   0.110
         0.000   0.000   1.000   0.100   0.002   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000  -0.000   0.120   0.000   0.000
         0.000   0.000   0.000   1.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   1.000   0.000   0.000   0.000   1.000
         0.000   0.000   0.000   0.000   1.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   1.000   1.000   1.000   0.000
         0.000   0.000   0.000   0.000   0.000   1.000   0.000   0.000   0.000   0.000   0.000   0.000   1.000   0.000   0.000   0.000   0.000   0.000

14.irteusgl$ format-array *m2*
         1.000   0.000   0.000   0.000   0.067   0.100  -0.000  -0.000  -0.808  -0.479  -0.150  -0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   1.000   0.000  -0.067  -0.000  -0.002  -0.002   0.808   0.000   0.000   0.000   0.110   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   1.000  -0.100   0.002   0.000   0.000   0.000  -0.000   0.120   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   1.000   0.000   0.000   0.000   1.000   0.000   0.000   0.000   1.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   0.000   1.000   0.000   0.000   0.000   1.000   1.000   1.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         0.000   0.000   0.000   0.000   0.000   1.000   1.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000
         1.000   0.000   0.000   0.000   0.067  -0.100   0.000   0.000   0.000   0.000   0.000   0.000  -0.000  -0.000  -0.808  -0.479  -0.150  -0.000
         0.000   1.000   0.000  -0.067  -0.000  -0.002   0.000   0.000   0.000   0.000   0.000   0.000  -0.002   0.808   0.000   0.000   0.000   0.110
         0.000   0.000   1.000   0.100   0.002   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000  -0.000   0.120   0.000   0.000
         0.000   0.000   0.000   1.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   1.000   0.000   0.000   0.000   1.000
         0.000   0.000   0.000   0.000   1.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   1.000   1.000   1.000   0.000
         0.000   0.000   0.000   0.000   0.000   1.000   0.000   0.000   0.000   0.000   0.000   0.000   1.000   0.000   0.000   0.000   0.000   0.000

となり値が一致しません胴体がroll軸正の方向に回転すれば、足はy軸正の方向に動くはずなので、ヤコビアンの2行目4列目が正である_m1_のが正しくなると思います。

(send robot :worldcoords) (send (car (send robot :links)) :worldcoords) の２つの値がちがう、というのが根本原因のように思います。

調べてみたところ、jskeus/irteus/demo/sample-robot-model.lにあったsample-robotは2つの値が一致していました。 hrpsys_ros_bridge_tutorials/models/の中にあったロボットモデルでは、2つの値が異なっていました(samplerobot jaxon_red hrp2jsk uratalegで確認)

1.irteusgl$ load "package://hrpsys_ros_bridge_tutorials/models/samplerobot.l"
t
4.irteusgl$ setq *robot* (instance samplerobot-robot :init)
#<samplerobot-robot #X63b2d88 SampleRobot  0.0 0.0 0.0 / 0.0 0.0 0.0>
5.irteusgl$ (send *robot* :worldcoords)
#<coordinates #X63b3fe8  0.0 0.0 0.0 / 0.0 0.0 0.0>
6.irteusgl$ (send (car (send *robot* :links)) :worldcoords)
#<coordinates #X66b64e0  0.0 0.0 723.5 / 0.0 0.0 0.0>
7.irteusgl$

この2つの値が違うのは https://github.com/start-jsk/rtmros_hrp2/blob/master/jsk_models/JAXON_RED/JAXON_REDmain.wrl#L166 でロボットのルートリンクの位置を床からの高さで設定しているからだと思います

(send *robot* :worldcoords)
(send (car (send *robot* :links)) :worldcoords)

この値は一致させたほうがいいのでしょうか

現在の実装では以下のような前提でirteus内で整合性が取れているため，このissueと https://github.com/euslisp/jskeus/issues/398 はcloseして良いのではないでしょうか．

with-append-root-joint
(output-link-list robot input-link-list :joint-class joint-class :joint-args joint-args)

で追加されたjointは，

$ (send (car (send robot :links)) :joint)
->追加された仮想joint
$ (send (car (send robot :links)) :parent-link)
->追加された親リンク
$ (send 追加された親リンク :child-links)
->((car (send robot :links)))

となることを除いて，

親リンク: (instance bodyset-link :init (make-cascoords)) ;;即ちworld座標系原点
子リンク: robot

であるかのように振る舞います．

また， world座標系原点を親リンク，robotを子リンクとする omiwheel-joint sphere-joint 6-dof-jointなどのヤコビアンは，以下のように表現されます．

関節変位の並進成分: そのjointが生成された当初のrobotの座標系で表現
関節変位の回転成分: 現在のrobotの位置を中心に回転する各軸ベクトル．そのjointが生成された当初のrobotの座標系で表現
エンドエフェクタ変位の並進成分: (デフォルトでは)現在のエンドエフェクタ座標系で表現
エンドエフェクタ変位の回転成分: 現在のエンドエフェクタの位置を中心に回転する各軸ベクトル.(デフォルトでは)現在のエンドエフェクタ座標系で表現

###########################################################

したがって，上で議論されているように，現在のロボットの位置 (send *robot* :worldcoords) 現在のルートリンクの位置 (send (car (send *robot* :links)) :worldcoords) が一致しない場合に，

関節変位の並進成分: そのjointが生成された当初のrobotの座標系で表現
関節変位の回転成分: 現在のルートリンクの位置を中心に回転する各軸ベクトル．そのjointが生成された当初のrobotの座標系で表現
エンドエフェクタ変位の並進成分: 現在のエンドエフェクタ座標系で表現
エンドエフェクタ変位の回転成分: 現在のエンドエフェクタの位置を中心に回転する各軸ベクトル.現在のエンドエフェクタ座標系で表現

となるようなヤコビアンを求めたい場合には，with-append-root-jointを使うことはできません．代わりに，以下のようにすると一致します．

1.irteusgl$ (load "irteus/demo/sample-robot-model.l")
2.irteusgl$ (setq *robot* (instance sample-robot :init))
3.irteusgl$ ;;https://gist.github.com/hikun5296/e611e5215c6c6d7e5b90e6828c62acdb をロード

4.irteusgl$ (setq *m1* (send *robot* :calc-jacobian-from-link-list-with-6dof
                 (mapcar #'(lambda (l) (send *robot* :link-list (send *robot* l :end-coords :parent))) '(:rleg :lleg))
                 :move-target (list (send *robot* :rleg :end-coords) (send *robot* :lleg :end-coords))
                 :translation-axis (list t t) :rotation-axis (list t t)))

5.irteusgl$ (setq *m2*
      (let ((vlink (instance bodyset-link :init (make-cascoords)
                             :bodies (list (make-cube 150 10 400))
                             :name 'virtual-link
                             :weight 0 :centroid (float-vector 0 0 0)
                             :inertia-tensor (make-matrix 3 3)))
            (rlink (car (send *robot* :links))))
        (send-message rlink bodyset-link :add-joint
                      (instance 6dof-joint :init
                                :child-link rlink ;;ここがwith-append-root-jointと異なる.この場合，仮想関節のjoint-angleを変えるとロボットとルートリンクの位置関係がおかしくなることに注意．
                                :parent-link vlink))
        (send rlink :add-parent-link vlink)
        (send vlink :add-child-links rlink) 
        (unwind-protect
             (send *robot* :calc-jacobian-from-link-list
                   (mapcar #'(lambda (l) (send *robot* :link-list (send *robot* l :end-coords :parent))) '(:rleg :lleg))       :move-target (list (send *robot* :rleg-end-coords) (send *robot* :lleg-end-coords))
                   :translation-axis (list t t) :rotation-axis (list t t))
          (send rlink :del-joint)
          (send rlink :del-parent-link)
          (send vlink :del-child-link rlink))))

6.irteusgl$ format-array (m- *m1* *m2*)
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000 
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000 
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000 
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000 
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000 
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000 
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000 
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000 
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000 
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000 
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000 
         0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000

############################################################ また，https://github.com/euslisp/jskeus/issues/398 で議論されているように，仮想関節の並進運動に対してヤコビアン行列が変わらないほうが正しいです．

トルクヤコビアンを求める場合には，関節iの:joint-angleが微小変化したときの関節jの回転中心の位置の変化を考える必要があるため， irteusのデフォルトのヤコビアンでは仮想関節の並進成分が微小変化したときに仮想関節の回転成分の回転中心の位置が変化する (追加した仮想リングが世界座標系の中に固定ではなくロボットといっしょに動いてしまう) ことを意識して実装する必要がありますが，これはirteusの問題というよりも，トルクヤコビアンを計算するライブラリの責任と思います．

euslisp / jskeus

with-append-root-jointにおいて仮想jointのchild-linkがrobotを指している #387