min_max_tableの扱いが関数によって異なる

[Naoki-Hiraoka]

:angle-vectorは、2つのjointのmin-max-table両方を見て、中間の整合性が取れる角度にセットします。

jskeus/irteus/irtmodel.l

Lines 845 to 868 in 6d5b288

	(t
	;;
	(do ((i 0 (incf i 0.1)))
	((> i 1))
	(setq tmp-joint-min-angle (send j :joint-min-max-table-min-angle tmp-target-joint-angle)
	tmp-joint-max-angle (send j :joint-min-max-table-max-angle tmp-target-joint-angle))
	(setq tmp-target-joint-min-angle (send jj :joint-min-max-table-min-angle tmp-joint-angle)
	tmp-target-joint-max-angle (send jj :joint-min-max-table-max-angle tmp-joint-angle))
	(if (< tmp-joint-angle tmp-joint-min-angle)
	(incf tmp-joint-angle (* (- tmp-joint-min-angle tmp-joint-angle) i)))
	(if (> tmp-joint-angle tmp-joint-max-angle)
	(incf tmp-joint-angle (* (- tmp-joint-max-angle tmp-joint-angle) i)))
	(if (< tmp-target-joint-angle tmp-target-joint-min-angle)
	(incf tmp-target-joint-angle (* (- tmp-target-joint-min-angle tmp-target-joint-angle) i)))
	(if (> tmp-target-joint-angle tmp-target-joint-max-angle)
	(incf tmp-target-joint-angle (* (- tmp-target-joint-max-angle tmp-target-joint-angle) i)))
	)
	(setq (j . joint-angle) tmp-joint-angle
	(jj . joint-angle) tmp-target-joint-angle)
	(setf (elt vec i) tmp-joint-angle)
	(setf (elt vec ji) tmp-target-joint-angle)
	)
	);; cond

一方で、
:rarm :angle-vectorは、単純にjoint-angleをセットするので、min-max-tableをバラバラにチェックしています。

jskeus/irteus/irtrobot.l

Lines 145 to 153 in 6d5b288

	(:angle-vector
	(if args
	(progn
	(mapcar #'(lambda (l a)
	(send l :joint :joint-angle a))
	(send self limb) (coerce (car args) cons))
	(send self limb :angle-vector))
	(coerce (mapcar #'(lambda (l) (send l :joint :joint-angle))
	(send self limb)) float-vector)))

:inverse-kinematicsの中で呼ばれる:move-jointsも、単純にjoint-angleをセットするので、min-max-tableをバラバラにチェックしています。

jskeus/irteus/irtmodel.l

Lines 1503 to 1538 in 6d5b288

	(:move-joints
	(union-vel &rest args
	&key
	union-link-list
	(periodic-time 0.05) ;; sec
	(joint-args)
	(debug-view nil)
	(move-joints-hook)
	&allow-other-keys)
	(let (dav dtheta j)
	(if (and debug-view (atom debug-view)) (setq debug-view (list debug-view)))
	(setq dav (send* self :calc-joint-angle-speed union-vel args))
	;; truncate to speed limit
	(let ((tmp-gain (send self :calc-joint-angle-speed-gain union-link-list dav periodic-time))
	(min-gain 1.0))
	(dotimes (i (length tmp-gain))
	(if (< (elt tmp-gain i) min-gain)
	(setq min-gain (elt tmp-gain i))))
	(setq dav (scale min-gain dav dav)))
	(when (and debug-view (not (memq :no-message debug-view)))
	(format-array (map float-vector #'rad2deg dav) "dav^ :"))
	;; update body
	(do ((i 0 (+ i (send j :joint-dof)))
	(l 0 (1+ l)))
	((>= l (length union-link-list)))
	(setq j (send (elt union-link-list l) :joint))
	(case (send j :joint-dof)
	(1 (setq dtheta (send j :speed-to-angle (elt dav i))))
	(t (setq dtheta (send j :speed-to-angle (subseq dav i (+ i (send j :joint-dof)))))))
	(send* (elt union-link-list l) :joint :joint-angle dtheta :relative t joint-args))
	(if move-joints-hook (funcall move-joints-hook))
	t))

min_max_tableの扱いが関数によって異なることによって、ロボットの姿勢が予期せず変わってしまうことがあるため、前者に統一したいです。