fulbody-inverse-kinematicsでmove-targetがルートリンクにassocされているとエラー

[Masaki Murooka]

:fullbody-inverse-kinematics
でmove-targetがルートリンクにassocされているとセグフォります．

[Masaki Murooka]

再現コード https://gist.github.com/mmurooka/9a3a28667777de87c5eeabffe73f0fed
を @snozawa さんに見て頂いて，
with-append-root-joint https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L3012
がいけなさそうというところまでわかりました．

[Shunichi Nozawa]

(defmacro with-append-root-joint (params &rest bodies)
  (with-gensyms
   (robot link-list vlink rlink joint-args)
   `(progn
      (let ((,robot ,(cadr params))
            (,link-list
             (if (every #'listp ,(caddr  params))
                 ,(caddr  params)
               (list ,(caddr  params))))
            (,joint-args ,(cadr (member :joint-args params))))
        (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* ,(or (cadr (member :joint-class params)) 6dof-joint) :init
                                   :child-link ,robot
                                   :parent-link ,vlink
                                   ,joint-args))
          (send ,rlink :add-parent-link ,vlink)
          (send ,vlink :add-child-links ,rlink)
          (unwind-protect
              (let ((,(car params) (mapcar #'(lambda (l)
                                               (cons ,rlink l))
                                           ,link-list)))
                ,@bodies)
            (send ,rlink :del-joint)
            (send ,rlink :del-parent-link)
            (send ,vlink :del-child-link ,rlink)))))))

ちょっと直し方考えますが、上記を試してみてください。

[Masaki Murooka]

testはセグフォでした．

13.irteusgl$ test
Call Stack (max depth: 20):
  0: at :cog-gain
  1: at :cog-gain
  2: at :cog-gain
  3: at :cog-gain
  4: at :cog-gain
  5: at :cog-gain
Segmentation fault (コアダンプ)

test2の結果

12.irteusgl$ test2
(nil
 ("LLEG_LINK1" "LLEG_LINK2" "LLEG_LINK3" "LLEG_LINK4" "LLEG_LINK5" "LLEG_LINK6")
 ("RLEG_LINK1" "RLEG_LINK2" "RLEG_LINK3" "RLEG_LINK4" "RLEG_LINK5" "RLEG_LINK6"))
((#<bodyset-link #X619df90 WAIST_LINK0  0.234 0.146 668.15 / 0.0 -0.001 -0.0>)
 (#<bodyset-link #X619df90 WAIST_LINK0  0.234 0.146 668.15 / 0.0 -0.001 -0.0>
  #<bodyset-link #X63a1000 LLEG_LINK1  0.216 90.146 668.128 / 0.0 -0.001 -0.0>
  #<bodyset-link #X63a5d10 LLEG_LINK2  0.216 90.146 668.128 / 0.0 -0.379 -0.0>
  #<bodyset-link #X6399d78 LLEG_LINK3  131.164 90.066 339.777 / 9.762e-05 -0.379 -0.0>
  #<bodyset-link #X638d4a0 LLEG_LINK4  131.164 90.066 339.777 / -0.0 0.453 -0.0>
  #<bodyset-link #X635c2b0 LLEG_LINK5  -0.018 90.0 69.978 / -2.712e-20 5.551e-17 -0.0>
  #<bodyset-link #X633fbe8 LLEG_LINK6  -0.018 90.0 69.978 / -2.712e-20 5.551e-17 -5.421e-20>)
 (#<bodyset-link #X619df90 WAIST_LINK0  0.234 0.146 668.15 / 0.0 -0.001 -0.0>
  #<bodyset-link #X61606e8 RLEG_LINK1  0.251 -89.854 668.172 / 0.0 -0.001 -0.0>
  #<bodyset-link #X6161990 RLEG_LINK2  0.251 -89.854 668.172 / 0.0 -0.379 -0.0>
  #<bodyset-link #X6162f68 RLEG_LINK3  131.199 -89.934 339.821 / 9.762e-05 -0.379 -0.0>
  #<bodyset-link #X63c23d8 RLEG_LINK4  131.199 -89.934 339.821 / -0.0 0.453 -0.0>
  #<bodyset-link #X63ba298 RLEG_LINK5  0.018 -90.0 70.022 / -2.712e-20 5.551e-17 -0.0>
  #<bodyset-link #X63bac28 RLEG_LINK6  0.018 -90.0 70.022 / -2.712e-20 5.551e-17 -5.421e-20>))
nil

[Masaki Murooka]

コンパイルされている中で使われているマクロは，
再定義しても反映されていないのではないかということで，
(load "package://jskeus/irteus/irtrobot.l")
してから(test)を呼んだらできました．ありがとうございます．

[Masaki Murooka]

closed via #446