fullbody ik root link end-coords error

fullbody-ik-root-link-end-coords-error.l

(defun test
  ()
  (load "package://hrpsys_ros_bridge_tutorials/euslisp/samplerobot-interface.l")
  (setq *robot* (instance samplerobot-robot :init))
  (send *robot* :fix-leg-to-coords (make-coords))

  ;; add additional end-coords
  (let* ((parent-link
          (elt (send *robot* :links) 0))
         )
    (send *robot* :put :additional-end-coords
          (make-cascoords :coords (send parent-link :copy-worldcoords)
                          :parent parent-link))
    )

  ;; fullbody ik
  (let* ((mt
          (append (list (send *robot* :get :additional-end-coords))
                  (send *robot* :legs :end-coords)))
         (tc
          (send-all mt :copy-worldcoords))
         (ll
          (mapcar #'(lambda (c) (send *robot* :link-list (send c :parent))) mt))
         )
     (send *robot* :fullbody-inverse-kinematics
           tc
           :move-target mt
           :link-list ll
           :centroid-thre 100
           :debug-view :no-message
           )))
    )

(defun test2
  ()
  (load "package://hrpsys_ros_bridge_tutorials/euslisp/samplerobot-interface.l")
  (setq *robot* (instance samplerobot-robot :init))
  (send *robot* :fix-leg-to-coords (make-coords))

  ;; add additional end-coords
  (let* ((parent-link
          (elt (send *robot* :links) 0))
         )
    (send *robot* :put :additional-end-coords
          (make-cascoords :coords (send parent-link :copy-worldcoords)
                          :parent parent-link))
    )

  (let* ((mt
          (append (list (send *robot* :get :additional-end-coords))
                  (send *robot* :legs :end-coords)))
         (tc
          (send-all mt :copy-worldcoords))
         (ll
          (mapcar #'(lambda (c) (send *robot* :link-list (send c :parent))) mt))
         )
    (pprint (mapcar #'(lambda (l)
                        (send-all l :name))
                    ll))
    (with-append-root-joint ;; inverse-kinematics with base-link
     (link-list-with-robot-6dof *robot* ll
                                :joint-class 6dof-joint)
     (pprint link-list-with-robot-6dof)
     ))
  )

irtmodel.lをロードしてtestをした結果です．

1.irteusgl$ test
;; extending gcstack 0x69f6840[32738] --> 0x7493f00[65476] top=7fe1
Call Stack (max depth: 20):
  0: at (send-all union-link-list :joint)
  1: at (send-all union-link-list :joint)
  2: at (send-all union-link-list :joint)
  3: at (append (send-all union-link-list :joint) joint-list)
  4: at (remove-duplicates (append (send-all union-link-list :joint) joint-list))
  5: at (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :joint-angle)
  6: at (let* ((loop 0) (union-link-list (if (functionp union-link-list) (funcall union-link-list link-list) (send self :calc-union-link-list link-list))) (av0 (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :joint-angle)) (c0 (unless (send self :parent) (send self :copy-worldcoords))) (target-coords0 target-coords) dif-pos dif-rot (success t) (old-analysis-level (send-all union-link-list :analysis-level)) ik-args collision-status command-directory command-filename command-id initial-target-coords initial-av initial-log-str) (send-all union-link-list :analysis-level :coords) (when (or (null link-list) (null move-target)) (warn ";; ERROR: :link-list or :move-target required~%") (return-from :inverse-kinematics t)) (if (and (null translation-axis) (null rotation-axis)) (return-from :inverse-kinematics t)) (when dump-command (setq initial-log-str (format nil ";; ik ~A log at ~A on ~A~%;;~%" (if success "success" "fail") (string-trim '(10) (unix:asctime (unix:localtime))) lisp-implementation-version)) (setq initial-log-str (format nil "~A;; link-list ~A~%" initial-log-str link-list)) (setq initial-log-str (format nil "~A;; move-target ~A~%" initial-log-str move-target)) (setq initial-log-str (format nil "~A;; rotatoin-axis ~A, translation-axis ~A~%" initial-log-str rotation-axis translation-axis)) (setq initial-log-str (format nil "~A;; thre ~A, rthre ~A, stop ~A~%" initial-log-str thre rthre stop)) (if (atom target-coords) (setq initial-target-coords (cons 'setq (cons 'c0 (list target-coords)))) (setq initial-target-coords (cons 'setq (cons 'c0 (list (cons 'quote (list (mapcar #'(lambda (x) x) target-coords)))))))) (setq initial-av (cons 'setq (cons 'av0 (list (send self :angle-vector)))))) (when debug-view (if (atom debug-view) (setq debug-view (list debug-view))) (when (memq :no-clear debug-view) (push :no-clear debug-view)) (when (memq :no-flush debug-view) (push :no-flush debug-view))) (if (atom (car link-list)) (setq link-list (list link-list))) (if (atom move-target) (setq move-target (list move-target))) (if (atom target-coords) (setq target-coords (list target-coords))) (if (atom rotation-axis) (setq rotation-axis (list rotation-axis))) (if (atom translation-axis) (setq translation-axis (list translation-axis))) (if (atom thre) (setq thre (list thre))) (if (atom rthre) (setq rthre (list rthre))) (if (and additional-jacobi (or (atom additional-jacobi) (functionp additional-jacobi))) (setq additional-jacobi (list additional-jacobi))) (if (and additional-vel (or (atom additional-vel) (functionp additional-vel))) (setq additional-vel (list additional-vel))) (unless (= (length translation-axis) (length rotation-axis) (length move-target) (length link-list) (length target-coords)) (warn ";; ERROR: list length differ : translation-axis ~A rotation-axis ~A move-target ~A link-list ~A target-coords ~A~%" (length translation-axis) (length rotation-axis) (length move-target) (length link-list) (length target-coords)) (return-from :inverse-kinematics t)) (unless (= (length additional-jacobi) (length additional-vel)) (warn ";; ERROR: list length differ : additional-jacobi ~A additional-vel ~A~%" (length additional-jacobi) (length additional-vel)) (return-from :inverse-kinematics t)) (let ((tmp-additional-jacobi (mapcar #'(lambda (aj) (if (functionp aj) (funcall aj link-list) aj)) additional-jacobi))) (setq ik-args (nconc (send* self :inverse-kinematics-args :translation-axis translation-axis :rotation-axis rotation-axis :additional-jacobi-dimension (+ (if (and (not cog-null-space) target-centroid-pos) (case cog-translation-axis (t 3) ((:x :y :z :xx :yy :zz) 2) ((:xy :yx :yz :zy :zx :xz) 1) (nil 0)) 0) (reduce #'+ (mapcar #'(lambda (aj) (array-dimension (if (functionp aj) (funcall aj link-list) aj) 0)) tmp-additional-jacobi))) :union-link-list union-link-list args) args))) (send self :reset-joint-angle-limit-weight-old union-link-list) (when (memq dump-command '(:always-with-debug-log :fail-only-with-debug-log)) (send self :put :ik-target-error (append (let ((tmp-count -1)) (mapcar #'(lambda (x) (list (read-from-string (format nil ":target-~d" (incf tmp-count))))) (make-list (length target-coords)))) (if target-centroid-pos (list (list :centroid))) (let ((tmp-count -1)) (mapcar #'(lambda (x) (list (read-from-string (format nil ":additional-~d" (incf tmp-count))))) (make-list (length additional-vel))))))) (while (< (incf loop) stop) (let* ((target-coords (mapcar #'(lambda (x) (if (functionp x) (funcall x) x)) target-coords)) (dif-pos (mapcar #'(lambda (mv tc trans-axis) (send mv :difference-position tc :translation-axis trans-axis)) move-target target-coords translation-axis)) (dif-rot (mapcar #'(lambda (mv tc rot-axis) (send mv :difference-rotation tc :rotation-axis rot-axis)) move-target target-coords rotation-axis))) (setq success (send* self :inverse-kinematics-loop dif-pos dif-rot :target-coords target-coords :periodic-time periodic-time :stop stop :loop loop :rotation-axis rotation-axis :translation-axis translation-axis :move-target move-target :union-link-list union-link-list :thre thre :rthre rthre :additional-jacobi additional-jacobi :additional-vel additional-vel :debug-view debug-view :ik-args ik-args args)) (when (eq success :ik-succeed) (setq success t) (return nil)))) (if target-centroid-pos (send self :update-mass-properties)) (let* ((target-coords (mapcar #'(lambda (x) (if (functionp x) (funcall x) x)) target-coords))) (setq dif-pos (mapcar #'(lambda (mt tc ta) (send mt :difference-position tc :translation-axis ta)) move-target target-coords translation-axis) dif-rot (mapcar #'(lambda (mt tc ra) (send mt :difference-rotation tc :rotation-axis ra)) move-target target-coords rotation-axis) success (send self :ik-convergence-check success dif-pos dif-rot rotation-axis translation-axis thre rthre centroid-thre target-centroid-pos centroid-offset-func cog-translation-axis :update-mass-properties nil))) (mapcar #'(lambda (l a) (send l :analysis-level a)) union-link-list old-analysis-level) (send self :reset-joint-angle-limit-weight-old union-link-list) (if check-collision (setq collision-status (send self :self-collision-check))) (setq success (and (or success (not revert-if-fail)) (not collision-status))) (when (or (memq dump-command '(:always :always-with-debug-log t)) (and (memq dump-command '(:fail-only :fail-only-with-debug-log)) (not success))) (setq command-directory (format nil "/tmp/irtmodel-ik-~A" (unix:getpid)) command-id (let ((lt (unix:localtime)) (gtd (cadr (unix:gettimeofday)))) (substitute 48 32 (format nil "~A-~04d-~02d-~02d-~02d-~02d-~02d-~A-~A-~A" (send (class self) :name) (+ 1900 (elt lt 5)) (+ 1 (elt lt 4)) (elt lt 3) (elt lt 2) (elt lt 1) (elt lt 0) (subseq (format nil "~0,6d" gtd) 0 2) (subseq (format nil "~0,6d" gtd) 2 4) (subseq (format nil "~0,6d" gtd) 4)))) command-filename (format nil "~A/~A-~A.l" command-directory command-id (if success "success" "failure"))) (unix:mkdir command-directory) (with-open-file (f command-filename :direction :output) (format f "~A" initial-log-str) (dump-structure f initial-target-coords) (dump-structure f initial-av)) (if (memq dump-command '(:fail-only-with-debug-log :always-with-debug-log)) (with-open-file (f (format nil "~A/~A-~A-debug-log.l" command-directory command-id (if success "success" "failure")) :direction :output) (format f "(setq *ik-loop* ~A~%" loop) (format f "      *ik-target-error* '~A~%" (mapcar #'(lambda (x) (append (list (car x)) (reverse (cdr x)))) (send self :get :ik-target-error))) (format f "      *ik-target-error-thre* '~A)~%" (append (let ((tmp-count -1)) (mapcar #'(lambda (tr rtr) (list (read-from-string (format nil ":target-~d" (incf tmp-count))) (list (* 0.001 tr) rtr))) thre rthre)) (if target-centroid-pos (list (list :centroid (* 0.001 centroid-thre)))))) (remprop self :ik-target-error)))) (if success (send self :angle-vector) (progn (when warnp (warn ";; inverse-kinematics failed.~%") (dotimes (i (length move-target)) (warn ";; dif-pos : ~a/(~a/~a)~%" (elt dif-pos i) (norm (elt dif-pos i)) (elt thre i)) (warn ";; dif-rot : ~a/(~a/~a)~%" (elt dif-rot i) (norm (elt dif-rot i)) (elt rthre i))) (if target-centroid-pos (let ((dif-cog (send self :difference-cog-position target-centroid-pos :centroid-offset-func centroid-offset-func :translation-axis cog-translation-axis :update-mass-properties nil))) (warn ";; cog-dif : ~a/(~a/~a)~%" dif-cog (norm dif-cog) centroid-thre))) (warn ";;  coords : ~a~%" (send (let ((p self)) (while (send p :parent) (setq p (send p :parent))) p) :worldcoords)) (warn ";;  angles : ~a~%" av0) (if check-collision (warn ";;  collision : ~a~%" collision-status)) (warn ";;    args : ~a~%" (append (list target-coords) args)) (when dump-command (let (i print-args command-init command-setup command-args) (labels ((escape-string (name) (if (stringp name) (format nil "\"~A\"" name) name)) (escape-list (lst idx) (when (consp (elt lst idx)) (setf (elt lst idx) (append '(list) (elt lst idx))) (dotimes (i (length (elt lst idx))) (escape-list (elt lst idx) i)))) (escape-robot-link (lst idx) (when (and (derivedp (elt lst idx) bodyset-link) (send self :link (send (elt lst idx) :name))) (setf (elt lst idx) (cons 'send (cons 'r (cons ':link (list (escape-string (send (elt lst idx) :name)))))))) (if (consp (elt lst idx)) (dotimes (i (length (elt lst idx))) (escape-robot-link (elt lst idx) i)))) (transformation-list-until-links (acoords) (if (find (send acoords :parent) (send self :links)) (list acoords) (append (transformation-list-until-links (send acoords :parent)) (list acoords)))) (get-move-target-transform-list (mcoords) (let ((move-target-transformation-list (transformation-list-until-links mcoords))) (cons 'let* (cons (cons (cons 'p (list (cons 'send (cons 'r (cons ':link (list (escape-string (send (send (car move-target-transformation-list) :parent) :name)))))))) (list (cons 'mt (list (cons 'make-cascoords (cons ':coords (list (cons 'send (cons (cons 'send (cons 'p (list ':copy-worldcoords))) (cons ':transform (list (cons 'make-coords (cons ':4x4 (list (send (send (send (car move-target-transformation-list) :parent) :transformation (car (last move-target-transformation-list))) :4x4))))))))))))))) (cons (cons 'send (cons 'p (cons ':assoc (list 'mt)))) (list 'mt)))))) (deep-copy-list (lst) (mapcar #'(lambda (l) (if (consp l) (deep-copy-list l) l)) lst))) (setq print-args (deep-copy-list args)) (dotimes (i (/ (length print-args) 2)) (unless (eq (elt print-args (* 2 i)) :move-target) (escape-list print-args (+ 1 (* 2 i))))) (dotimes (i (length print-args)) (escape-robot-link print-args i)) (dotimes (j (count :move-target print-args)) (if (setq i (position :move-target print-args :count (1+ j))) (cond ((atom (elt print-args (+ i 1))) (setf (elt print-args (+ i 1)) (get-move-target-transform-list (elt print-args (+ i 1))))) (t (setf (elt print-args (+ i 1)) (append '(list) (mapcar #'(lambda (x) (get-move-target-transform-list x)) (elt print-args (+ i 1))))))))) (setq command-init (cons 'instance (cons (send (class self) :name) (list ':init))) command-setup (cons 'progn (cons (cons 'send (cons 'r (cons ':newcoords (list (cons 'make-coords (cons ':4x4 (list (send self :4x4)))))))) (cons (cons 'mapc (cons (cons 'function (list (cons 'lambda (cons (cons 'j (list 'a)) (list (cons 'send* (cons 'j (cons ':joint-angle (cons 'a (list joint-args)))))))))) (cons (cons 'list (mapcar #'(lambda (x) (cons 'send (cons 'r (list (intern (string-upcase x) *keyword-package*))))) (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :name))) (list (list 'quote av0))))) (list (cons 'objects (list (cons 'list (list 'r))))))))) (setq command-args (append '(list) (list (append (mapcar #'(lambda (x) (cons 'make-coords (cons ':pos (cons (send (if (functionp x) (funcall x) x) :worldpos) (cons ':rot (list (send (if (functionp x) (funcall x) x) :worldrot))))))) target-coords) nil)) (list :dump-command nil :debug-view t) print-args)) (setf (elt command-args 1) (append '(list) (elt command-args 1)))) (warn ";; command : ~a~%" (cons 'let (cons (list (cons 'r (list command-init))) (cons command-setup (list (cons 'send* (cons 'r (cons ':inverse-kinematics (list command-args))))))))) (warn ";; dump debug command to ~A~%" command-filename) (warn ";; (progn (load \"~A\")(ik-setup)(ik-check))~%" command-filename) (with-open-file (f command-filename :direction :output :if-exists :append) (format f "(defun ~A-setup () (let ((r ~A)) (setq *robot* r) ~A (objects (list *robot*))))~%" command-id command-init command-setup) (format f "(defun ~A-check () (let ((r *robot*)) (send* r :inverse-kinematics ~A)))~%" command-id command-args) (format f "(defun ik-setup () (~A-setup))~%" command-id) (format f "(defun ik-check () (~A-check))~%" command-id) (format f "(setq ik-failed '(") (format f "(:dif-pos . ~A) (:dif-rot . ~A~%)" dif-pos dif-rot) (if target-centroid-pos (format f "(:dif-cog . ~A)" (send self :difference-cog-position target-centroid-pos :centroid-offset-func centroid-offset-func :translation-axis cog-translation-axis :update-mass-properties nil))) (format f "))~%"))))) (mapc #'(lambda (j a) (send* j :joint-angle a joint-args)) (remove-duplicates (append (send-all union-link-list :joint) joint-list)) av0) (if c0 (send self :newcoords c0)) nil)))
  7: at :cog-gain
  8: at :cog-gain
  9: at :cog-gain
  10: at :cog-gain
;; Segmentation Fault.
;; in (send-all union-link-list :joint)
;; You are still in a signal handler.
;;Try reset or throw to upper level as soon as possible.
;; code=-2008159760 x=884de8c0 addr=7fa612001390
Fatal:   C-c C-cinterrupt2.B1-irteusgl$ 
nil
2.B1-irteusgl$ ,union-link-list
(#<bodyset-link #X6ee7c30 WAIST_LINK0  0.234 0.146 668.15 / 0.0 -0.001 -0.0> nil (#<bodyset-link #X6854788 LLEG_LINK1  0.216 90.146 668.128 / 0.0 -0.001 -0.0> #<bodyset-link #X659a030 LLEG_LINK2  0.216 90.146 668.128 / 0.0 -0.379 -0.0> #<bodyset-link #X659cd18 LLEG_LINK3  131.164 90.066 339.777 / 9.762e-05 -0.379 -0.0> #<bodyset-link #X659cf10 LLEG_LINK4  131.164 90.066 339.777 / -0.0 0.453 -0.0> #<bodyset-link #X65a33f0 LLEG_LINK5  -0.018 90.0 69.978 / -2.712e-20 5.551e-17 -0.0> #<bodyset-link #X65a64c8 LLEG_LINK6  -0.018 90.0 69.978 / -2.712e-20 5.551e-17 -5.421e-20>) (#<bodyset-link #X6c6fcf0 RLEG_LINK1  0.251 -89.854 668.172 / 0.0 -0.001 -0.0> #<bodyset-link #X6d89ca8 RLEG_LINK2  0.251 -89.854 668.172 / 0.0 -0.379 -0.0> #<bodyset-link #X6d0cf70 RLEG_LINK3  131.199 -89.934 339.821 / 9.762e-05 -0.379 -0.0> #<bodyset-link #X6c9c8f0 RLEG_LINK4  131.199 -89.934 339.821 / -0.0 0.453 -0.0> #<bodyset-link #X6ca0748 RLEG_LINK5  0.018 -90.0 70.022 / -2.712e-20 5.551e-17 -0.0> #<bodyset-link #X6c90430 RLEG_LINK6  0.018 -90.0 70.022 / -2.712e-20 5.551e-17 -5.421e-20>))
3.B1-irteusgl$ ,link-list
((#<bodyset-link #X6ee7c30 WAIST_LINK0  0.234 0.146 668.15 / 0.0 -0.001 -0.0> nil (#<bodyset-link #X6854788 LLEG_LINK1  0.216 90.146 668.128 / 0.0 -0.001 -0.0> #<bodyset-link #X659a030 LLEG_LINK2  0.216 90.146 668.128 / 0.0 -0.379 -0.0> #<bodyset-link #X659cd18 LLEG_LINK3  131.164 90.066 339.777 / 9.762e-05 -0.379 -0.0> #<bodyset-link #X659cf10 LLEG_LINK4  131.164 90.066 339.777 / -0.0 0.453 -0.0> #<bodyset-link #X65a33f0 LLEG_LINK5  -0.018 90.0 69.978 / -2.712e-20 5.551e-17 -0.0> #<bodyset-link #X65a64c8 LLEG_LINK6  -0.018 90.0 69.978 / -2.712e-20 5.551e-17 -5.421e-20>) (#<bodyset-link #X6c6fcf0 RLEG_LINK1  0.251 -89.854 668.172 / 0.0 -0.001 -0.0> #<bodyset-link #X6d89ca8 RLEG_LINK2  0.251 -89.854 668.172 / 0.0 -0.379 -0.0> #<bodyset-link #X6d0cf70 RLEG_LINK3  131.199 -89.934 339.821 / 9.762e-05 -0.379 -0.0> #<bodyset-link #X6c9c8f0 RLEG_LINK4  131.199 -89.934 339.821 / -0.0 0.453 -0.0> #<bodyset-link #X6ca0748 RLEG_LINK5  0.018 -90.0 70.022 / -2.712e-20 5.551e-17 -0.0> #<bodyset-link #X6c90430 RLEG_LINK6  0.018 -90.0 70.022 / -2.712e-20 5.551e-17 -5.421e-20>)))