cascaded-linkのjoint-listやlinksの要素を追加・削除したい

[r-tanaka3]

cascaded-linkでは、joint-listやlinksを:init以外で追加したり、削除したりすることは可能でしょうか。
具体的には、以下のコードで、(send (send (car (cdr (cdr (send r :links)))) :parent) :dissoc (car (cdr (cdr (send r :links)))))で:dissocしたリンクl3を、:locateで移動させた後、l3をそこに残したまま残りのリンクl2だけを動かしたいです。現状では、:dissocと:locateまでは期待通りの動きができるのですが、その後に(send r :angle-vector (float-vector (- 45 i) 0))としたときに:dissocしたリンクl3が元の位置に戻ってしまいます。

(defclass fridge
  :super cascaded-link)
(defmethod fridge
  (:init ()
	 (let (outer box tray tmp l1 l2 l3)
	   (send-super :init)
	   (setq outer (make-cube 50 50 50))
	   (setq box (make-cube 45 47 45))
	   (send box :locate #f(0 2 0))
	   (setq outer (body- outer box))
	   (send outer :set-color :lavender)
	   (setq tmp (make-cube 45 47 10))
	   (send tmp :locate #f(0 2 17.5))
	   (setq box (body- box tmp))
	   (setq tmp (make-cube 40 42 33))
	   (send tmp :locate #f(0 2 2))
	   (setq box (body- box tmp))
	   (setq tmp (make-cube 50 2 50))
	   (send tmp :locate #f(0 25.5 0))
	   (setq box (body+ box tmp))
	   (send box :set-color :SteelBlue)
	   (setq tray (make-cube 45 47 10))
	   (setq tmp (make-cube 40 42 8))
	   (send tmp :locate #f(0 0 2))
	   (setq tray (body- tray tmp))
	   (send tray :locate #f(0 2 17.5))
	   (send tray :set-color :aquamarine)
	   (setq l1 (instance bodyset-link :init (make-cascoords) :bodies (list outer)))
	   (setq l2 (instance bodyset-link :init (make-cascoords) :bodies (list box)))
	   (setq l3 (instance bodyset-link :init (make-cascoords) :bodies (list tray)))
	   (send self :assoc l1)
	   (send l1 :assoc l2)
	   (send l2 :assoc l3)
	   (setq joint-list
		 (list (instance linear-joint
				 :init :parent-link l1 :child-link l2
				 :axis :y)
		       (instance linear-joint
				 :init :parent-link l2 :child-link l3
				 :axis :y)))
	   (setq links (list l1 l2 l3))
	   (send self :init-ending)
	   )))
(defun take-out-tray nil
  (let (r)
    (setq r (instance fridge :init))
    (objects (list r))
    (dotimes (i 45)
      (send r :angle-vector (float-vector i 0))
      (send *irtviewer* :draw-objects)
      (x::window-main-one))
    (dotimes (i 40)
      (send r :angle-vector (float-vector 45 (- 0 i)))
      (send *irtviewer* :draw-objects)
      (x::window-main-one))
    (dotimes (i 40)
      (send r :angle-vector (float-vector 45 (+ -40 i)))
      (send *irtviewer* :draw-objects)
      (x::window-main-one))
    (send (send (car (cdr (cdr (send r :links)))) :parent) :dissoc (car (cdr (cdr (send r :links)))))
    (dotimes (i 15)
      (send (car (cdr (cdr (send r :links)))) :locate #f(0 0 1))
      (send *irtviewer* :draw-objects)
      (x::window-main-one))
    (dotimes (i 45)
      (send r :angle-vector (float-vector (- 45 i) 0))
      (send *irtviewer* :draw-objects)
      (x::window-main-one))
    ))

[Naoki-Hiraoka]

:initと:init-endingで行っている処理の逆を行えば、cascaded-linkのjoint-listやlinksの要素を削除することは可能です。

jskeus/irteus/irtmodel.l

Lines 790 to 802 in 69be3cf

	(:init-ending
	()
	"This method is to called finalize the instantiation of the cascaded-link. This update bodies and child-link and parent link from joint-list"
	(setq bodies (flatten (send-all links :bodies)))
	(dolist (j joint-list)
	(send (send j :child-link) :add-joint j)
	(send (send j :child-link) :add-parent-link (send j :parent-link))
	(send (send j :parent-link) :add-child-links (send j :child-link))
	)
	(send self :update-descendants)
	;; Make default collision shape
	(dolist (l (send self :links)) (send l :make-collisionmodel))
	)

次のようにすれば、:dissocしたリンクl3が元の位置に戻らなくなります。

(defun take-out-tray nil
  (let (r l)
    (setq r (instance fridge :init))
    (objects (list r))
    (dotimes (i 45)
      (send r :angle-vector (float-vector i 0))
      (send *irtviewer* :draw-objects)
      (x::window-main-one))
    (dotimes (i 40)
      (send r :angle-vector (float-vector 45 (- 0 i)))
      (send *irtviewer* :draw-objects)
      (x::window-main-one))
    (dotimes (i 40)
      (send r :angle-vector (float-vector 45 (+ -40 i)))
      (send *irtviewer* :draw-objects)
      (x::window-main-one))
    (setq l (elt (send r :links) 2))
    (send (send l :parent) :dissoc l)
    (delete (send l :joint) (send r :joint-list))
    (send l :del-joint)
    (send (send l :parent-link) :del-child-link l)
    (send l :del-parent-link)
    (delete l (send r :links))
    (dotimes (i 15)
      (send l :locate #f(0 0 1))
      (send *irtviewer* :draw-objects)
      (x::window-main-one))
    (dotimes (i 45)
      (send r :angle-vector (float-vector (- 45 i) 0))
      (send *irtviewer* :draw-objects)
      (x::window-main-one))
    ))

ただ、cascaded-linkのjoint-listやlinksの要素を追加・削除することはあまり想定されておらず、容易に操作できるようにはなっていないため、初めからl3をcascaded-linkとは別に定義しておいて、:assoc/:dissocのみでfridgeと接続・非接続を切り替えるようにするのが簡単と思います。

[r-tanaka3]

できるようになりました。ありがとうございます。