min-max-tableのある関節を:inverse-kinematicsすると、各イテレーションごとに:angle-vectorがわずかに振動する.

[Naoki-Hiraoka]

情報共有のためのissueです.

min-max-tableのある関節を:inverse-kinematicsすると、:inverse-kinematicsの各イテレーションごとに:angle-vectorがわずかに振動します. そのため、操縦などで、数十~数百Hzで:inverse-kinematicsを解き直して短い補間時間で:angle-vectorを実機に逐次的に指令するような用途で使う場合には、注意が必要です.

関節角度上下限は不等式制約ですが、sr-inverseに基づく逆運動学は不等式制約が扱えないので、:inverse-kinematicsでは前回のイテレーションで関節角度がlimitに近づいたならば、その関節のweightを大きくして動かしにくくする、というアプローチがとられています.

jskeus/irteus/irtmodel.l

Lines 1577 to 1603 in 3040a1a

	;;
	;; wmat/weight: weighting joint angle weight
	;;
	;; w_i = 1 + | dH/dt | if d|dH/dt| >= 0
	;; = 1 if d|dH/dt| < 0
	;; dH/dt = (t_max - t_min)^2 (2t - t_max - t_min)
	;; / 4 (t_max - t)^2 (t - t_min)^2
	;;
	;; T. F. Chang and R.-V. Dubey: "A weighted least-norm solution based
	;; scheme for avoiding joint limits for redundant manipulators", in IEEE
	;; Trans. On Robotics and Automation, 11((2):286-292, April 1995.
	;;
	(when (> avoid-weight-gain 0.0)
	(setq joint-angle-limit-weight
	(scale avoid-weight-gain
	(joint-angle-limit-weight (send-all union-link-list :joint) tmp-len) tmp-len))
	(when (and debug-view (not (memq :no-message debug-view)))
	(format-array joint-angle-limit-weight-old "ocost :")
	(format-array joint-angle-limit-weight "cost :"))
	(dotimes (i fik-len)
	(setf (elt tmp-weight i)
	(if (>= (- (elt joint-angle-limit-weight i)
	(elt joint-angle-limit-weight-old i)) 0.0)
	(/ 1.0 (+ 1.0 (elt joint-angle-limit-weight i)))
	1.0))
	(setf (elt joint-angle-limit-weight-old i) (elt joint-angle-limit-weight i))
	))

しかし、min-max-tableのある関節ではlimitの側も動くためか、関節角度がlimitに近づくのと遠ざかるのを交互にくり返す現象が発生することがあり、関節のweightが各イテレーションで急激に変動することで解の:angle-vectorが振動してしまうようです。

「前回のイテレーションで関節角度がlimitに近づいたならば、その関節のweightを大きくして動かしにくくする」機能を削除すると、振動しなくなりました。

自分はirteusでこの現象に遭遇したのではなく、同じアルゴリズムが採用されている別の逆運動学ソルバでこの現象に遭遇したのですが、同じアルゴリズムを使っているのでirteusでもこの現象が発生するだろうと思いこちらに共有させていただきました。