Skip to content

Commit

Permalink
Merge pull request jsk-ros-pkg#636 from mmurooka/update-ik-grid
Browse files Browse the repository at this point in the history 
[jsk_ik_server] Add centroid method to ik-grid
  • Loading branch information
@k-okada
k-okada authored Oct 13, 2016
2 parents 2606e24 + 7d98b4e commit 56e18af
Showing 1 changed file with 59 additions and 47 deletions.
106 changes: 59 additions & 47 deletions jsk_ik_server/euslisp/ik-evaluation.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,18 @@
))))))
(:cells () cells)
(:valid-cells () (remove-if #'(lambda (x) (= (send x :get-value) 0)) cells))
(:centroid
()
(vector-mean
(mapcar #'(lambda (c) (send origin :transform-vector (send c :center-pos)))
(send self :cells)))
)
(:valid-centroid
()
(vector-mean
(mapcar #'(lambda (c) (send origin :transform-vector (send c :center-pos)))
(send self :valid-cells)))
)
(:dump-to-csv (fname)
(with-open-file (f fname :direction :output)
(format f "x,y,z,i,j,k,value~%")
Expand Down Expand Up @@ -425,56 +437,56 @@
(fullbody nil)
(initial-pose nil))
(let ((counter 0))
(dolist (cell (send grid :cells))
(if initial-pose (send robot :angle-vector initial-pose) (send robot :reset-pose))
(if fullbody (send robot :fix-leg-to-coords (make-coords)))
(if (< (distance2d (send robot :worldpos) (send cell :center-pos)) min-distance)
(dolist (cell (send grid :cells))
(if initial-pose (send robot :angle-vector initial-pose) (send robot :reset-pose))
(if fullbody (send robot :fix-leg-to-coords (make-coords)))
(if (< (distance2d (send robot :worldpos) (send cell :center-pos)) min-distance)
(progn
(warn "skip because it violtates min-distance~%")
(send cell :set-value 0))
(progn
(warn "skip because it violtates min-distance~%")
(send cell :set-value 0))
(progn
(when orient-centerp
(send grid :put :center-pos center-pos)
(setq rotation
(send (orient-coords-to-axis (make-coords :pos (send cell :center-pos)) (v- center-pos (send cell :center-pos))
(if (cadr (member :rotation-axis ik-args)) (cadr (member :rotation-axis ik-args)) :z)) :rot)))
(if (if fullbody
(send* robot :fullbody-inverse-kinematics
(append (list (make-coords :pos (send cell :center-pos)
:rot rotation))
(send-all (send robot :legs :end-coords) :copy-worldcoords))
:move-target (append (list move-target) (send robot :legs :end-coords))
:link-list (mapcar #'(lambda (mt) (send robot :link-list (send mt :parent))) (append (list move-target) (send robot :legs :end-coords)))
(when orient-centerp
(send grid :put :center-pos center-pos)
(setq rotation
(send (orient-coords-to-axis (make-coords :pos (send cell :center-pos)) (v- center-pos (send cell :center-pos))
(if (cadr (member :rotation-axis ik-args)) (cadr (member :rotation-axis ik-args)) :z)) :rot)))
(if (if fullbody
(send* robot :fullbody-inverse-kinematics
(append (list (make-coords :pos (send cell :center-pos)
:rot rotation))
(send-all (send robot :legs :end-coords) :copy-worldcoords))
:move-target (append (list move-target) (send robot :legs :end-coords))
:link-list (mapcar #'(lambda (mt) (send robot :link-list (send mt :parent))) (append (list move-target) (send robot :legs :end-coords)))
:dump-command nil
:warnp nil
ik-args)
(send* robot :inverse-kinematics
(make-coords :pos (send cell :center-pos)
:rot rotation)
:move-target move-target
:link-list (send robot :link-list (send move-target :parent))
:dump-command nil
:warnp nil
ik-args)
(send* robot :inverse-kinematics
(make-coords :pos (send cell :center-pos)
:rot rotation)
:move-target move-target
:link-list (send robot :link-list (send move-target :parent))
:dump-command nil
:warnp nil
ik-args))
(send cell :set-value 1)
(send cell :set-value 0))
(when update-view
(if (= (send cell :get-value) 1)
(objects (send cell :cube)))
(send *irtviewer* :draw-objects)
(x::window-main-one))
(incf counter)
(when (= (mod counter 10) 0)
(format t "~c~0,4d/~A" #x0d counter (length (send grid :cells)))
(let* ((percentage (/ (* 100 counter) (length (send grid :cells))))
(max-bar-length 60)
(bar-num (/ (* max-bar-length percentage) 100)))
(dotimes (i bar-num)
(format t "=")))
(finish-output))
)))
(send grid :normalize-cells)
))
ik-args))
(send cell :set-value 1)
(send cell :set-value 0))
(when update-view
(if (= (send cell :get-value) 1)
(objects (send cell :cube)))
(send *irtviewer* :draw-objects)
(x::window-main-one))
(incf counter)
(when (= (mod counter 10) 0)
(format t "~c~0,4d/~A" #x0d counter (length (send grid :cells)))
(let* ((percentage (/ (* 100 counter) (length (send grid :cells))))
(max-bar-length 60)
(bar-num (/ (* max-bar-length percentage) 100)))
(dotimes (i bar-num)
(format t "=")))
(finish-output))
)))
(send grid :normalize-cells)
))

(defun save-4-views (file-prefix)
;; slant
Expand Down

0 comments on commit 56e18af

Please sign in to comment.