JAXONのhead-y/pのmin-max-tableの更新

[YoheiKakiuchi]

現状に即して、min-max-tableを更新する

[YoheiKakiuchi]

今回、計測してもらったデータ

(setq pairs '(
(8.362 48);;フレーム
(11.487 44.444);;フレーム
(16.137 41.181);;フレーム
(21.312 36.956);;ひも
(26 32.319);;ひも
(28.256 26.850);;ひも
(30.687 15.037);;ひも
(33.975 8.069);;ひもの角（ここを超えると一気にヨーの可動範囲が広がる）
(60.887 7.137);;後頭部
(61.294 -18.181);;クレーン
(33.712 -18.237);;クレーン
(-2.650 -22.231);;クレーン
(-2.650 48);; additional for closed polygon
))

整形すると添付のような範囲になります。
赤点がこれまでの範囲、緑点が新しい範囲です。
(x軸がyaw, y軸がpitch)

min-max tableの修正は以下の通り。

      (send (send self :joint "HEAD_JOINT0") :joint-min-max-table
            '#2f((-21 -20 -19 -18 -17 -16 -15 -14 -13 -12 -11 -10 -9 -8 -7 -6 -5 -4 -3 -2 -1 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48) (-8 -17 -26 -61 -61 -61 -61 -61 -61 -61 -61 -61 -61 -61 -61 -61 -61 -61 -61 -61 -61 -61 -60 -60 -60 -60 -60 -60 -60 -35 -33 -33 -32 -32 -31 -31 -30 -30 -30 -30 -29 -29 -29 -29 -29 -28 -28 -28 -28 -27 -27 -26 -26 -26 -25 -24 -23 -22 -21 -20 -18 -17 -16 -14 -13 -12 -10 -10 -9 -8) (8 17 26 61 61 61 61 61 61 61 61 61 61 61 61 61 61 61 61 61 61 61 60 60 60 60 60 60 60 35 33 33 32 32 31 31 30 30 30 30 29 29 29 29 29 28 28 28 28 27 27 26 26 26 25 24 23 22 21 20 18 17 16 14 13 12 10 10 9 8))
                              )
      (send (send self :joint "HEAD_JOINT0") :joint-min-max-target (send self :joint "HEAD_JOINT1"))

      (send (send self :joint "HEAD_JOINT1") :joint-min-max-table
            '#2f((-61 -60 -59 -58 -57 -56 -55 -54 -53 -52 -51 -50 -49 -48 -47 -46 -45 -44 -43 -42 -41 -40 -39 -38 -37 -36 -35 -34 -33 -32 -31 -30 -29 -28 -27 -26 -25 -24 -23 -22 -21 -20 -19 -18 -17 -16 -15 -14 -13 -12 -11 -10 -9 -8 -7 -6 -5 -4 -3 -2 -1 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61) (-18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -19 -19 -19 -19 -19 -19 -19 -19 -19 -20 -20 -20 -20 -20 -20 -20 -20 -20 -21 -21 -21 -21 -21 -21 -21 -21 -21 -21 -21 -21 -21 -21 -21 -21 -21 -20 -20 -20 -20 -20 -20 -20 -20 -20 -19 -19 -19 -19 -19 -19 -19 -19 -19 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18 -18) (0 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 8 8 10 12 14 18 23 27 29 32 33 34 35 36 37 38 38 39 40 41 41 42 43 44 44 46 47 48 48 48 48 48 48 48 48 48 48 48 48 48 48 48 48 48 47 46 44 44 43 42 41 41 40 39 38 38 37 36 35 34 33 32 29 27 23 18 14 12 10 8 8 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 7 0))
                              )
      (send (send self :joint "HEAD_JOINT1") :joint-min-max-target (send self :joint "HEAD_JOINT0"))

プログラムベタ書きです。

;; 計測データからpolygonを作る
(setq poly (instance polygon :init :vertices
                     (mapcar #'(lambda (x) (float-vector (car x) (cadr x) 0)) pairs)))

;; x-y平面上のpolygonからmin-maxのデータを作る
(defun print-min-max-polygon (poly)
  (let* ((angle-pairs (mapcar #'(lambda (p) (list (elt p 0) (elt p 1))) (send poly :vertices)))
         (x-max (apply #'max (mapcar #'(lambda (x) (car x)) angle-pairs)))
         (x-min (apply #'min (mapcar #'(lambda (x) (car x)) angle-pairs)))
         (y-max (apply #'max (mapcar #'(lambda (x) (cadr x)) angle-pairs)))
         (y-min (apply #'min (mapcar #'(lambda (x) (cadr x)) angle-pairs)))
         xrange yrange
         xret yret
         )
    (let ((st (ceiling x-min))
          (len (- (floor   x-max)
                  (ceiling x-min))))
      (dotimes (i (1+ len))
        (push (+ i st) xrange)))
    (let ((st (ceiling y-min))
          (len (- (floor   y-max)
                  (ceiling y-min))))
      (dotimes (i (1+ len))
        (push (+ i st) yrange)))
    (setq xrange (nreverse xrange))
    (setq yrange (nreverse yrange))
    (format t ";;; x ;;;~%")
    (dolist (x xrange)
      (let ((yline (make-line (float-vector x -1000 0) (float-vector x 1000 0)))
            pt)
        (dolist (e (send poly :edges))
          (let ((param (car (send e :intersection yline))))
            (unless (or (c-isnan param)
                        (> param 1)
                        (< param 0))
              (push (send e :point param) pt)))
          )
        (when (> (length pt) 1)
          (push (list x
                      (apply #'max (mapcar #'(lambda (x) (elt x 1)) pt))
                      (apply #'min (mapcar #'(lambda (x) (elt x 1)) pt))) xret)
          (format t "~A ~A ~A~%"
                  x
                  (apply #'max (mapcar #'(lambda (x) (elt x 1)) pt))
                  (apply #'min (mapcar #'(lambda (x) (elt x 1)) pt))
                  ))

        ))
    (format t ";;; y ;;;~%")
    (dolist (y yrange)
      (let ((xline (make-line (float-vector -1000 y 0) (float-vector 1000 y 0)))
            pt)
        (dolist (e (send poly :edges))
          (let ((param (car (send e :intersection xline))))
            (unless (or (c-isnan param)
                        (> param 1)
                        (< param 0))
              (push (send e :point param) pt)))
          )
        (when (> (length pt) 1)
          (push (list y
                      (apply #'max (mapcar #'(lambda (x) (elt x 0)) pt))
                      (apply #'min (mapcar #'(lambda (x) (elt x 0)) pt))) yret)
          (format t "~A ~A ~A~%"
                  y
                  (apply #'max (mapcar #'(lambda (x) (elt x 0)) pt))
                  (apply #'min (mapcar #'(lambda (x) (elt x 0)) pt))
                  ))

        ))
    (list xret yret)
    ))

(let ((r (print-min-max-polygon poly))) (setq xret (car r)) (setq yret (cadr r)))

;; 今回は半分しか計測してないので
;; yaw軸は0以上のデータを正として対称にする
(setq xret (remove-if #'(lambda (x) (< (car x) 0)) xret))
(setq xret
(remove nil
        (append
         xret
         (mapcar #'(lambda (x) (when (> (car x) 0)
                                 (list (- (car x)) (cadr x) (caddr x)))) xret))))
(sort xret #'(lambda (x y) (> (car x) (car y))))

;; おなじく
;; pitchに対するyaw軸は符号違いの同じ絶対値を持つとする
(setq yret
(mapcar #'(lambda (x)
            (list (car x) (cadr x) (- (cadr x))))
        yret))

;; table形式にする
(let (xlst
      maxlst
      minlst)
(dolist (x xret)
  (let ((xx    (car x))
        (max-y (cadr x))
        (min-y (caddr x)))
    (setq min-y (ceiling min-y))
    (setq max-y (floor   max-y))
    (unless (>= min-y max-y)
      (push xx xlst)
      (push max-y maxlst)
      (push min-y minlst)
      )
    ))
(list xlst minlst maxlst))

;; table形式にする
(let (ylst
      maxlst
      minlst)
(dolist (y yret)
  (let ((yy    (car y))
        (max-x (cadr y))
        (min-x (caddr y)))
    (setq min-x (ceiling min-x))
    (setq max-x (floor   max-x))
    (unless (>= min-x max-x)
      (push yy ylst)
      (push max-x maxlst)
      (push min-x minlst)
      )
    ))
(list ylst minlst maxlst))

min-max-tableのプロット方法

(load "/home/leus/prog/euslib/jsk/gnuplotlib.l")
(load "package://hrpsys_ros_bridge_tutorials/euslisp/jaxon-interface.l")
(setq *robot* (jaxon))

;; old
;;(send (send *robot* :joint "HEAD_JOINT0") :put :org-min-angle -23.0)
;;(send (send *robot* :joint "HEAD_JOINT0") :put :org-max-angle  23.0)
;;(send (send *robot* :joint "HEAD_JOINT1") :put :org-min-angle -32.0)
;;(send (send *robot* :joint "HEAD_JOINT1") :put :org-max-angle  39.0)

;; new
(send (send *robot* :joint "HEAD_JOINT0") :put :org-min-angle -61.0)
(send (send *robot* :joint "HEAD_JOINT0") :put :org-max-angle  61.0)
(send (send *robot* :joint "HEAD_JOINT1") :put :org-min-angle -21.0)
(send (send *robot* :joint "HEAD_JOINT1") :put :org-max-angle  48.0)

(let (xx yy
         bxx byy)
  (dolist (p (send *robot* :plot-joint-min-max-table-common
                   (send *robot* :head :neck-y)
                   (send *robot* :head :neck-p)))
    (push (car p) xx) (push (cadr p) yy))
  #|(dolist (p (send *robot* :plot-joint-min-max-table-common
                   (send *robot* :head :neck-p)
                   (send *robot* :head :neck-y)))
    (push (car p) bxx) (push (cadr p) byy))|#

  (graph-view (list yy) xx :mode "points")
  ;;(graph-view (list bxx) byy :mode "points")
  )

[98shimpei]

頭にRealSenseを取り付けたため、min-max-tableを更新しました。しばらく運用して問題なさそうであればPRします。以下今回計測した点とmin-max-tableのグラフです。
計測した点

(setq pairs '((0 -46) (-47 -43) (-47 0) (-40 0) (-37 2) (-37 20) (-20 20) (-16 22) (-16 29) (0 29) (16 29) (16 22) (20 20) (37 20) (37 2) (40 0) (47 0) (47 -43)))

更新後のmin-max-tableのグラフ

[98shimpei]

RealSenseにカバーを取り付けたため再度min-max-tableを更新しました。以下今回計測した点とmin-max-tableのグラフです。

(setq pairs '((0 -46) (-47 -43) (-47 0) (-37 0) (-34 2) (-34 20) (-18 20) (-15 22) (-15 29) (0 29) (15 29) (15 22) (18 20) (34 20) (34 2) (37 0) (47 0) (47 -43)))