Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Open
YoheiKakiuchi opened this issue Jun 28, 2017 · 3 comments
Open

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

YoheiKakiuchi opened this issue Jun 28, 2017 · 3 comments
Assignees
@yuki-shark

Comments

@YoheiKakiuchi
Copy link
Member

現状に即して、min-max-tableを更新する
@YoheiKakiuchi
Copy link
Member Author

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

(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)
image

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
Copy link

頭に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のグラフ
min-max-table

@98shimpei
Copy link

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)))

new-min-max-table

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees

@yuki-shark yuki-shark

Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
3 participants
@YoheiKakiuchi @yuki-shark @98shimpei