Skip to content

Instantly share code, notes, and snippets.

@furushchev

furushchev/drc-testbed-models.l

Created April 14, 2015 07:54
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save furushchev/ec3056aacdc340f41ce4 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save furushchev/ec3056aacdc340f41ce4 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
drc-testbed-models.l
;;;;;;;;;;;;;;;;
;; DRC testbed models
;; (snozawa, mmurooka, s-noda)
;;;;;;;;;;;;;;;;
#-:jsk
(jsk)
#-:rbrain-basic
(rbrain)
(load "~/prog/euslib/rbrain/convert-to-irtmodel.l")
;;;;;;;;;;;;;;;;
;; Utilities
;;;;;;;;;;;;;;;;
(defclass single-link-model
:super cascaded-link
:slots ()
)
(defmethod single-link-model
(:init
(&key (name) ((:bodies bs)))
(prog1
(send-super :init :name name)
(setq links (list (instance bodyset-link :init (make-cascoords)
:bodies bs :name :root-link)))
(send self :assoc (car links))
(send self :init-ending)
))
)
(defun make-cylinder-shell
(outer-radius inner-radius height)
(let ((b0 (make-cylinder outer-radius height))
(b1 (make-cylinder inner-radius (+ 1 height))))
(body- b0 b1)
))
;;;;;;;;;;;;;;;;
;; Model classes
;;;;;;;;;;;;;;;;
;; copied from jskmapclsas
(defclass valve
:super cascaded-link
:slots (handle valve-joint)
)
(defmethod valve
(:init
(&rest args
&key (thick 20) (radius 115)
(ring-color :gray10) (base-color :royalblue)
(type :round) (with-root-pipe t))
(send-super* :init args)
(setq handle nil)
;; 1. make links links and assoc all links
(let ((rl (send self :make-root-link
:base-color base-color :with-root-pipe with-root-pipe))
(vl (send self :make-valve-link
:thick thick :radius radius :ring-color ring-color :type type)))
(send vl :translate #f(0 0 353) :world) ;;vavle's hight (H)
;; 2. assoc links
;; Root link should be associated with "self".
(send self :assoc rl)
(send rl :assoc vl)
;; 3. make all joints
;; Before making joints, you should :assoc all links.
(setq valve-joint (instance rotational-joint :init
:parent-link rl :child-link vl
:name :crank-joint :axis :z
:min -3600 :max 3600))
;; 4. define slots for robot class
;; links and joint-list for cascaded-link.
(setq links (list rl vl))
(setq joint-list (list valve-joint))
;; 5. call :init-ending after defining links and joint-list and return "self"
(send self :init-ending)
self))
;; Methods to define robot links
(:make-root-link
(&key base-color with-root-pipe)
(let (base-cylinder
(r 90)
(L 240)
(H 353)
(thick 30)
tmp
cylinder1
cylinder2
cylinder3
cylinder4
upper-body
sphere
rl
)
;;base cylinder
(setq base-cylinder (make-cylinder r L))
(send base-cylinder :rotate (deg2rad 90) :x)
(send base-cylinder :locate (float-vector 0 (* 0.5 L) 0) :world)
(setq tmp (make-cube (* L 2) (- L (* thick 2)) (* L 2)))
(setq base-cylinder (body- base-cylinder tmp))
(setq sphere (make-gdome (make-icosahedron r)))
(setq cylinder1 (make-cylinder (* r 0.8) (- L (* 2 thick))))
(send cylinder1 :rotate (deg2rad 90) :x)
(send cylinder1 :locate (float-vector 0 (* 0.5 (- L (* 2 thick))) 0) :world)
;;
(setq tmp (make-cylinder (* r 0.5) (* L 2)))
(send tmp :rotate (deg2rad 90) :x)
(send tmp :locate (float-vector 0 0 (* -1 L)))
(setq base-cylinder (body- base-cylinder tmp))
(setq cylinder1 (body- cylinder1 tmp))
;;(setq sphere (body- sphere tmp))
;;
(setq cylinder2 (make-cylinder r thick))
(send cylinder2 :locate (float-vector 0 0 r))
(setq cylinder3 (make-cylinder (* r 0.8) (* thick 2)))
(send cylinder3 :locate (float-vector 0 0 (- r (* 2 thick))))
;;
(setq cylinder4 (make-cylinder 20 H))
;;(send cylinder4 :locate (float-vector 0 0 (* 0.5 H)))
;;upper body
(setq upper-body
(make-solid-of-revolution
(mapcar #'(lambda (x) (scale (* 0.7 (/ (- H r) 3.5)) x)) (list #f(0 0 3.5) #f(0.6 0 3) #f(1.0 0 2) #f(1.4 0 1) #f(1.0 0 0)))))
(send upper-body :locate (float-vector 0 0 (+ r thick)))
;;
(send upper-body :assoc sphere)
(send upper-body :assoc cylinder1)
(send upper-body :assoc cylinder2)
(send upper-body :assoc cylinder3)
(send upper-body :assoc cylinder4)
(setq cylinder5 (make-cylinder 45 (+ 500 1130) :pos (float-vector 0 1130 0) :rpy (float-vector 0 0 pi/2)))
(send upper-body :assoc cylinder5)
(send upper-body :assoc base-cylinder)
(setq rl (instance bodyset-link :init (make-cascoords)
:bodies (if with-root-pipe (list upper-body base-cylinder sphere cylinder1 cylinder2 cylinder3 cylinder4 cylinder5) (list upper-body cylinder4))
:name :crank-root-link))
(dolist (l (send rl :bodies))
(send l :set-color base-color))
rl
))
(:make-valve-link
(&key thick radius ring-color type)
(cond ((equal type :round)
(let* ((segments 16)
(ring-radius radius)
(pipe-radius thick)
(ring (make-ring ring-radius pipe-radius :segments segments))
(cross-bar1 (make-cube pipe-radius (* ring-radius 2) pipe-radius))
(cross-bar2 (make-cube (* ring-radius 2) pipe-radius pipe-radius)))
(send ring :assoc cross-bar1)
(send ring :assoc cross-bar2)
(let ((valve-link (instance bodyset-link :init (make-cascoords)
:bodies (list ring cross-bar1 cross-bar2) :name :valve-handle-link)))
;;
(push (make-cascoords :coords (send (send (send ring :copy-worldcoords)
:translate (float-vector 0 (* -1 radius) 0))
:rotate (/ pi 2) :y)
:name :valve-handle) handle)
(send valve-link :assoc (car handle))
(dolist (l (send valve-link :bodies))
(send l :set-color ring-color))
valve-link)))
((equal type :bar)
(let* ((segments 16)
(bar-thick (/ 27 2))
(bar-length 393)
(bar-root-thick (/ 45.5 2))
(bar-root-length 100)
(bar (make-cylinder bar-thick bar-length :segments segments))
(bar-root (make-cylinder bar-root-thick bar-root-length :segments segments)))
(send bar-root :assoc bar)
(send bar-root :rotate pi/2 :x :world)
(send bar-root :translate (float-vector 0 (/ bar-root-length 2.0) 0) :world)
(let ((valve-link (instance bodyset-link :init (make-cascoords)
:bodies (list bar-root bar) :name :valve-handle-link)))
;;
(push (make-cascoords :coords (send (send (send bar-root :copy-worldcoords)
:translate (float-vector 0 0 350))
:rotate (/ -pi 2) :z)
:name :valve-handle) handle)
(send valve-link :assoc (car handle))
(dolist (l (send valve-link :bodies))
(send l :set-color ring-color))
valve-link))))
)
(:handle (&rest args) (forward-message-to-all handle args))
(:valve-handle () (car handle))
(:valve-joint (&rest args) (forward-message-to valve-joint args))
)
(defclass drc-stair
:slots (stair-top-faces ground-body)
:super single-link-model
)
(defmethod drc-stair
(:init
(&key (add-groud-p nil))
(setq stair-top-faces nil)
(let* ((blist (append (send self :make-stair-bodies)
(send self :make-top-stair-bodies)
(send self :make-handrail-bodies)
(send self :make-small-handrail-bodies :l/r :right)
(send self :make-small-handrail-bodies)
(send self :make-top-handrail-bodies))))
(if add-groud-p
(let ((b (make-cube 1000 1200 10)))
(send b :translate (float-vector 0 0 -5))
(setq ground-body b)
(setq stair-top-faces (append stair-top-faces
(list (find-if #'(lambda (x) (memq :top (send x :id))) (send ground-body :faces)))))
(push b blist)))
(dolist (b blist) (send b :set-color :gray))
(dolist (b (cdr blist)) (send (car blist) :assoc b))
(send-super :init :name "drc-stair" :bodies blist)))
(:make-stair-bodies
()
(let ((s1 (make-cube 290 1080 225))
(s2 (make-cube 290 1080 230))
(s3 (make-cube 270 1080 230)))
(send s1 :translate (float-vector (/ 290 2.0) 0 (/ 225 2.0)) :world)
(send s2 :translate (float-vector (+ 290 (/ 290 2.0)) 0 (+ 225 (/ 230 2.0))) :world)
(send s3 :translate (float-vector (+ 290 290 (/ 270 2.0)) 0 (+ 225 230 (/ 230 2.0))) :world)
(setq stair-top-faces
(append stair-top-faces
(mapcar #'(lambda (b)
(find-if #'(lambda (x) (memq :top (send x :id))) (send b :faces)))
(list s1 s2 s3))))
(list s1 s2 s3)
))
(:make-top-stair-bodies
()
(let ((s1 (make-cube 1205 2430 85)))
(send s1 :translate (float-vector (+ 290 290 270 (/ 1205 2.0)) (+ 85 (+ (/ 1080 2.0) (- (/ 2430 2.0)))) (+ 225 230 230 (/ 85 2.0) -85 220)))
(setq stair-top-faces
(append stair-top-faces
(mapcar #'(lambda (b)
(find-if #'(lambda (x) (memq :top (send x :id))) (send b :faces)))
(list s1))))
(list s1)
))
(:make-small-handrail-bodies
(&key (l/r :left))
(let ((b0 (make-cylinder 20 100))
(b1 (make-cylinder 20 100))
(b2 (make-cylinder 20 195)))
(send b0 :rotate -pi/2 :y)
(send b1 :rotate -pi/2 :y)
(send b0 :translate (float-vector 0 0 (/ (+ 850 890) 2.0)) :world)
(send b1 :translate (float-vector 0 0 (/ (+ 1045 1005) 2.0)) :world)
(send b2 :translate (float-vector -100 0 850) :world)
(if (eq l/r :left)
(send-all (list b0 b1 b2) :translate (float-vector 0 (/ 1080 2.0) 0))
(send-all (list b0 b1 b2) :translate (float-vector 0 (/ 1080 -2.0) 0)))
(list b0 b1 b2)
))
(:make-handrail-bodies
()
(let ((bf1 (make-cube 50 40 (+ 225 835)))
(bf2 (make-cube 50 40 (+ 225 835)))
(br1 (make-cube 50 40 995))
(br2 (make-cube 50 40 995))
(hr1 (make-cylinder 20 1040))
(hl1 (make-cylinder 20 1040))
(hr2 (make-cylinder 20 1040))
(hl2 (make-cylinder 20 1040)))
(send bf1 :translate (float-vector (/ 50 -2.0) (+ (/ 40 -2.0) (/ 1080 2.0)) (/ (+ 225 835) 2.0)))
(send bf2 :translate (float-vector (/ 50 -2.0) (+ (/ 40 2.0) (/ 1080 -2.0)) (/ (+ 225 835) 2.0)))
(send br1 :translate (float-vector (+ 290 290 230 (/ 50 -2.0)) (+ (/ 40 -2.0) (/ 1080 2.0)) (+ 225 230 230 (/ 995 2.0))))
(send br2 :translate (float-vector (+ 290 290 230 (/ 50 -2.0)) (+ (/ 40 2.0) (/ 1080 -2.0)) (+ 225 230 230 (/ 995 2.0))))
;;(send hr1 :rotate (deg2rad (- 90 40)) :y)
;;(send hl1 :rotate (deg2rad (- 90 40)) :y)
(send hr1 :rotate (deg2rad (- 90 38)) :y)
(send hl1 :rotate (deg2rad (- 90 38)) :y)
(send hr2 :rotate (deg2rad (- 90 38)) :y)
(send hl2 :rotate (deg2rad (- 90 38)) :y)
(send hr1 :translate (float-vector -40 (+ (/ 40 -2.0) (/ 1080 2.0)) (+ 225 835 -20)) :world)
(send hl1 :translate (float-vector -40 (+ (/ 40 2.0) (/ 1080 -2.0)) (+ 225 835 -20)) :world)
(send hr2 :translate (float-vector -40 (+ (/ 40 -2.0) (/ 1080 2.0)) (+ 225 385 -20)) :world)
(send hl2 :translate (float-vector -40 (+ (/ 40 2.0) (/ 1080 -2.0)) (+ 225 385 -20)) :world)
(list bf1 bf2 br1 br2 hr1 hl1 hr2 hl2)
))
(:make-top-handrail-bodies
()
(let ((b1 (make-cube 35 50 1065))
(b2 (make-cube 35 50 1065))
(b3 (make-cube 35 50 1065))
(b4 (make-cube 35 50 1065))
(b5 (make-cube 35 50 1065))
(b6 (make-cube 35 50 1065))
;;
(b7 (make-cube 1205 50 40))
(b8 (make-cube 1205 50 40))
(b9 (make-cube 1205 50 40))
(b10 (make-cube 1205 50 40))
(b11 (make-cube 50 2430 40))
(b12 (make-cube 50 2430 40))
;;
(b13 (make-cube 1205 10 105))
(b14 (make-cube 1205 10 105))
)
(send b1 :translate (float-vector (+ 290 290 270 (/ 35 2.0)) (+ 85 (/ 1080 2.0) (/ 50 2.0)) (+ 225 230 230 220 (/ 1065 2.0))))
(send b2 :translate (float-vector (+ 290 290 270 (/ 35 -2.0) 1205) (+ 85 (/ 1080 2.0) (/ 50 2.0)) (+ 225 230 230 220 (/ 1065 2.0))))
(send b3 :translate (float-vector (+ 290 290 270 (/ 35 -2.0) 1205) (+ 85 (/ 1080 2.0) (/ 50 -2.0) -2430) (+ 225 230 230 220 (/ 1065 2.0))))
(send b4 :translate (float-vector (+ 290 290 270 (/ 35 2.0)) (+ 85 (/ 1080 2.0) (/ 50 -2.0) -2430) (+ 225 230 230 220 (/ 1065 2.0))))
(send b5 :translate (float-vector (+ 290 290 270 1205 (/ 35 2.0)) (+ 85 -100 (/ 1080 2.0) (/ 50 2.0)) (+ 225 230 230 220 (/ 1065 2.0))))
(send b6 :translate (float-vector (+ 290 290 270 1205 (/ 35 2.0)) (+ 85 100 (/ 1080 2.0) (/ 50 -2.0) -2430) (+ 225 230 230 220 (/ 1065 2.0))))
;;
(send b7 :translate (float-vector (+ 290 290 270 (/ 1205 2.0)) (+ 85 (/ 1080 2.0) (/ 50 2.0)) (+ 225 230 230 220 1065 (/ 40 2.0))))
(send b8 :translate (float-vector (+ 290 290 270 (/ 1205 2.0)) (+ 85 (/ 1080 2.0) (/ 50 2.0)) (+ 225 230 230 220 550 (/ 40 2.0))))
(send b9 :translate (float-vector (+ 290 290 270 (/ 1205 2.0)) (+ 85 (/ 1080 2.0) (/ 50 -2.0) -2430) (+ 225 230 230 220 1065 (/ 40 2.0))))
(send b10 :translate (float-vector (+ 290 290 270 (/ 1205 2.0)) (+ 85 (/ 1080 2.0) (/ 50 -2.0) -2430) (+ 225 230 230 220 550 (/ 40 2.0))))
(send b11 :translate (float-vector (+ 290 290 270 1205 (/ 50 2.0)) (+ 85 (/ 1080 2.0) (/ 2430 -2.0)) (+ 225 230 230 220 1065 (/ 40 2.0))))
(send b12 :translate (float-vector (+ 290 290 270 1205 (/ 50 2.0)) (+ 85 (/ 1080 2.0) (/ 2430 -2.0)) (+ 225 230 230 220 550 (/ 40 2.0))))
;;
(send b13 :translate (float-vector (+ 290 290 270 (/ 1205 2.0)) (+ 85 (/ 1080 2.0) (/ 10 -2.0) -2430) (+ 225 230 230 220 (/ 40 2.0))))
(send b14 :translate (float-vector (+ 290 290 270 (/ 1205 2.0)) (+ 85 (/ 1080 2.0) (/ 10 -2.0) 0) (+ 225 230 230 220 (/ 40 2.0))))
(list b1 b2 b3 b4 b5 b6
b7 b8 b9 b10 b11 b12
b13 b14)
))
(:get-all-stair-top-faces
()
stair-top-faces)
)
(defclass drc-hose-wall
:super single-link-model
:slots ()
)
(defmethod drc-hose-wall
(:init
()
(let ((blist (send self :make-hose-wall-bodies)))
(dolist (b (cdr blist)) (send (car blist) :assoc b))
(send-super :init :name "drc-hose-wall" :bodies blist)))
(:make-hose-consent
()
(let ((b1 (make-cube 75 65 165))
(b2 (make-cylinder-shell (/ 56 2.0) (/ 48 2.0) 25.0))
(b3 (make-cylinder-shell (/ 56 2.0) (/ 48 2.0) 25.0))
(blist))
(send b1 :translate (float-vector (/ 75 -2.0) 0 0))
(send b2 :rotate pi/2 :y)
(send b2 :translate (float-vector (+ -75 -25) 0 (+ (/ 165 2.0) -40)) :world)
(send b3 :translate (float-vector (/ 75 -2.0) 0 (+ (/ 165 -2.0) -25.0)) :world)
(let ((blist (list b1 b2 b3)))
(send-all blist :set-color :gray60)
(dolist (b (cdr blist)) (send (car blist) :assoc b))
blist
)))
(:make-hose-wall-bodies
()
(let ((b0 (make-cube 10 1210 2430))
(blist1 (send self :make-hose-consent))
(blist2 (send self :make-hose-consent))
(blist))
(send b0 :set-color #F(0.8 0.5 0))
(send b0 :translate (float-vector (/ 10 2.0) 0 (/ 2430 2.0)))
(send (car blist1) :translate (float-vector 0 200 (+ (/ 165 2.0) 1120)))
(send (car blist2) :translate (float-vector 0 -200 (+ (/ 165 2.0) 1120)))
(send b0 :assoc (car blist1))
(send b0 :assoc (car blist2))
(append (list b0) blist1 blist2)))
)
(defclass drc-hose-plug
:super single-link-model
:slots ()
)
(defmethod drc-hose-plug
(:init
()
(let ((blist (send self :make-hose-plug-bodies)))
(dolist (b (cdr blist)) (send (car blist) :assoc b))
(send-super :init :name "drc-hose-plug" :bodies blist)))
(:make-hose-plug-bodies
()
(let ((b1 (make-cylinder (/ 45 2.0) 85.0))
(b2 (make-cube 20 75 10)))
(send b1 :rotate pi/2 :y)
(send b2 :translate (float-vector (/ 20 2.0) 0 0))
(send b1 :assoc b2)
(send-all (list b1 b2) :set-color :gray20)
(list b1 b2)
))
)
(defclass drc-drill-wall
:super single-link-model
:slots ()
)
(defmethod drc-drill-wall
(:init
()
(let ((blist (send self :make-drill-wall-bodies)))
(dolist (b (cdr blist)) (send (car blist) :assoc b))
(send-super :init :name "drc-drill-wall" :bodies blist)))
(:make-drill-wall-bodies
()
(let ((b1 (make-cube 10 1210 2430))
(b2 (make-cube 10 1210 2430))
(b2- (make-cube 20 810 810))
(b3 (make-cube 250 440 10))
b21 b22 b23 b24)
(send b2- :translate (float-vector 0 0 (+ 720 (/ 810 2.0) (/ 2430 -2.0))))
(setq b2 (body- b2 b2-))
(setq b21 (make-cube 10 (/ (- 1210 810) 2) 2430 :pos (float-vector 0 (+ (/ 810 2) (/ (- 1210 810) 4)) 0))
b22 (make-cube 10 850 720 :pos (float-vector 0 0 (+ (/ 2430 -2) (/ 710 2))))
b23 (make-cube 10 (/ (- 1210 810) 2) 2430 :pos (float-vector 0 (- (+ (/ 810 2) (/ (- 1210 810) 4))) 0))
b24 (make-cube 10 850 720 :pos (float-vector 0 0 (+ (/ 2430 2) (/ 710 -2)))))
(objects (list
b2 ))
(objects (list
b2
b21 b22 b23 b24))
(dolist (b (list b22 b23 b24))
(send b21 :assoc b))
(send b1 :translate (float-vector (/ 10 2.0) 0 (/ 2430 2.0)) :world)
(send b2 :translate (float-vector (/ 10 2.0) 0 (/ 2430 2.0)) :world)
(send b21 :translate (float-vector (/ 10 2.0) 0 (/ 2430 2.0)) :world)
(send b3 :translate (float-vector (/ 250 -2.0) 0 (+ (/ 10 2.0) 1020)))
(send b1 :assoc b3)
(send b1 :rotate (deg2rad 45) :z)
(send b2 :rotate (deg2rad -45) :z)
(send b21 :rotate (deg2rad -45) :z)
(send b1 :translate (float-vector 0 (* 1210 0.5) 0))
(send b2 :translate (float-vector 0 (* 1210 -0.5) 0))
(send b21 :translate (float-vector 0 (* 1210 -0.5) 0))
(send b21 :translate (float-vector 370 150 0))
(send-all (list b1 b2 b3 b21 b22 b23 b24) :set-color #f(0.8 0.5 0))
(list b1 b21 b22 b23 b24 b3)
;(list b1 b2 b3)
))
)
(defclass drc-terrain
:super cascaded-link
:slots (block-dimensions block-bodies ground-body block-angle)
)
(defmethod drc-terrain
(:init
(&rest args &key (name "drc-terrain") ((:block-dimensions bd) (list 390 195 140)) (add-groud-p nil)
((:block-angle ba) 15.0) ;; [deg]
&allow-other-keys)
(prog1
(send-super* :init :name name args)
(setq block-dimensions bd)
(setq block-angle ba)
(let* ((blist (send self :make-drc-terrain-block-bodies))
(l))
(if add-groud-p
(let ((b (make-cube 3700 3300 10)))
(send b :translate (float-vector 1200 1050 -5))
(send b :set-color :gray)
(setq ground-body b)
(send (car blist) :assoc b)
(setq blist (append blist (list b)))))
(setq l (instance bodyset-link :init (make-cascoords)
:name :root-link
:bodies blist))
(send self :assoc l)
(setq links (list l))
(setq joint-list (list))
(send self :init-ending)
self)))
(:make-drc-block-one
()
(let* ((block-x (elt block-dimensions 0))
(block-y (elt block-dimensions 1))
(block-z (elt block-dimensions 2))
(block-hole-x 170)
(block-hole-y 200)
(block-hole-z 85)
(block-body (make-cube block-x block-y block-z))
(block-hole1 (make-cube block-hole-x block-hole-y block-hole-z))
(block-hole2 (make-cube block-hole-x block-hole-y block-hole-z))
)
(send block-hole1 :translate (float-vector (/ (+ block-hole-x (/ (- block-x (* block-hole-x 2)) 3.0)) 2.0) 0 0) :local)
(send block-hole2 :translate (float-vector (- (/ (+ block-hole-x (/ (- block-x (* block-hole-x 2)) 3.0)) 2.0)) 0 0) :local)
(setq block-body (body- block-body block-hole1))
(setq block-body (body- block-body block-hole2))
(send block-body :set-color :gray)
block-body
)
)
(:make-drc-block-set-one
()
(let* ((block-x (elt block-dimensions 0))
(block-y (elt block-dimensions 1))
(block-z (elt block-dimensions 2))
(block1 (send self :make-drc-block-one))
(block2 (send self :make-drc-block-one))
(base-x 90)
(base-y block-x)
(base-z 90)
(base (make-cube base-x base-y base-z))
)
(send block1 :translate (float-vector 0 (/ block-y 2.0) (+ (/ block-z 2.0) (* (/ block-x 2.0) (sin (deg2rad block-angle))))) :local)
(send block2 :translate (float-vector 0 (- (/ block-y 2.0)) (+ (/ block-z 2.0) (* (/ block-x 2.0) (sin (deg2rad block-angle))))) :local)
(send base :translate (float-vector (- (- (/ block-x 2.0) (/ base-x 2.0))) 0 (/ base-x 2.0)) :local)
(send block1 :rotate (deg2rad block-angle) :y :local)
(send block2 :rotate (deg2rad block-angle) :y :local)
(send base :set-color :darkgoldenrod)
(send block1 :assoc block2)
(send block1 :assoc base)
(instance bodyset :init (make-cascoords) :bodies (list block1 block2 base))
)
)
(:make-drc-terrain-block-bodies
()
(let* ((orientation-map (list (list 1 2 3 0 1 2)
(list 2 3 0 1 2 3)
(list 3 0 1 2 3 0)
(list 0 1 2 3 0 1)
(list 1 2 3 0 1 2)
(list 2 3 0 1 2 3)
(list 3 0 1 2 3 0)))
(height-map (list (list 0 0 0 0 0 0)
(list 0 0 1 1 0 0)
(list 0 0 1 1 0 0)
(list 0 1 1 1 1 0)
(list 1 2 1 1 2 1)
(list 1 1 1 1 1 1)
(list 0 0 0 0 0 0))))
(let* ((block-set-region-x 400)
(block-set-region-y 400))
(dotimes (i (length orientation-map))
(dotimes (j (length (elt orientation-map i)))
(let* ((block-set (send self :make-drc-block-set-one)))
(send block-set :rotate (* (elt (elt orientation-map i) j) pi/2) :z :world)
(send block-set :translate (float-vector (* i block-set-region-x) (* j block-set-region-y) (* (elt (elt height-map i) j) (elt block-dimensions 2))) :world)
(push block-set block-bodies)))))
(setq block-bodies (flatten (send-all block-bodies :bodies)))
(dolist (b (cdr block-bodies)) (send (car block-bodies) :assoc b))
block-bodies)
)
(:block-bodies () block-bodies)
(:get-terrain-top-face-from-block-idx
(block-idx)
(list (elt (send (elt block-bodies (* 3 block-idx)) :faces) 6)
(elt (send (elt block-bodies (+ (* 3 block-idx) 1)) :faces) 6))
)
(:get-all-terrain-top-faces
()
(let ((ret -1))
(flatten (append (mapcar #'(lambda (x) (send self :get-terrain-top-face-from-block-idx (incf ret))) (make-list (/ (length block-bodies) 3)))
(list (find-if #'(lambda (x) (memq :top (send x :id))) (send ground-body :faces)))))))
)
(defclass drc-surprise-task-shower
:super cascaded-link
:slots (bar1-joint bar2-joint)
)
(defmethod drc-surprise-task-shower
(:init
()
(prog1
(send-super :init :name "drc-surprise-task-shower")
;; 1. make links links and assoc all links
(let ((l0 (send self :make-root-link))
(l1 (send self :make-bar1-link))
(l2 (send self :make-bar2-link)))
(send l1 :translate (float-vector (+ -465 190) 600 (+ 1650 190 545)))
(send l2 :translate (float-vector -465 600 (+ 1650 190 545)))
;; 2. assoc links
;; Root link should be associated with "self".
(send self :assoc l0)
(send l0 :assoc l1)
(send l1 :assoc l2)
;; 3. make all joints
;; Before making joints, you should :assoc all links.
(setq bar1-joint (instance rotational-joint :init
:parent-link l0 :child-link l1
:axis :y
:name :bar1-joint
:min -90 :max 90))
(setq bar2-joint (instance rotational-joint :init
:parent-link l1 :child-link l2
:axis :y
:name :bar2-joint
:min -90 :max 90))
;; 4. define slots for robot class
;; links and joint-list for cascaded-link.
(setq links (list l0 l1 l2))
(setq joint-list (list bar1-joint bar2-joint))
;; 5. call :init-ending after defining links and joint-list and return "self"
(send self :init-ending)
self)))
;; Methods to define robot links
(:make-root-link
()
(let* ((b0 (make-cube 10 1330 1590))
(b1 (make-cube (+ 465 150) 10 10))
(b2-height (- (+ 1650 190 545) 1590))
(b2 (make-cube 10 10 b2-height))
(b3 (make-torus (list (float-vector 0 0 0) (float-vector 0 100 -100) (float-vector 0 0 -100)))))
(send b0 :translate (float-vector (/ 10 2.0) 0 (/ 1800 2.0)))
(send b1 :translate (float-vector (/ (+ 465 150) -2.0) 600 (+ 1650 190 545)))
(send b2 :translate (float-vector (/ 10 -2.0) 600 (+ 1650 190 545 (/ b2-height -2.0))))
(send b3 :translate (float-vector (+ -465 -150) 600 (+ 1650 190 545)))
(send b0 :assoc b1)
(send b0 :assoc b2)
(send b0 :assoc b3)
(send b0 :set-color #f(0.8 0.5 0))
(send b1 :set-color :gray40)
(send b2 :set-color :gray60)
(send b3 :set-color :red)
(instance bodyset-link :init (make-cascoords) :bodies (list b0 b1 b2 b3))
))
(:make-bar1-link
()
(let ((b0 (make-cube 190 10 10)))
(send b0 :translate (float-vector (/ 190 -2.0) -10 0))
(send b0 :set-color :gray70)
(instance bodyset-link :init (make-cascoords) :bodies (list b0))
))
(:make-bar2-link
()
(let ((b0 (make-cube 10 10 545))
(b1 (make-prism (list (float-vector 0 0 0) (float-vector 100 190 0) (float-vector -100 190 0)) 10)))
(send b0 :translate (float-vector 0 0 (/ 545 -2.0)))
(send b1 :rotate -pi/2 :x)
(send b1 :translate (float-vector 0 0 -545) :world)
(send b0 :assoc b1)
(send b0 :set-color :gray60)
(send b1 :set-color :gray50)
(instance bodyset-link :init (make-cascoords) :bodies (list b0 b1))
))
)
(defclass drc-surprise-task-button
:super cascaded-link
:slots (hinge-joint)
)
(defmethod drc-surprise-task-button
(:init
()
(prog1
(send-super :init :name "drc-surprise-task-button")
;; 1. make links links and assoc all links
(let ((l0 (send self :make-root-link))
(l1 (send self :make-door-link)))
(send l1 :translate (float-vector -130 (/ 200 2.0) 1330))
;; 2. assoc links
;; Root link should be associated with "self".
(send self :assoc l0)
(send l0 :assoc l1)
;; 3. make all joints
;; Before making joints, you should :assoc all links.
(setq hinge-joint (instance rotational-joint :init
:parent-link l0 :child-link l1
:axis :z
:name :hinge-joint
:min -90 :max 90))
;; 4. define slots for robot class
;; links and joint-list for cascaded-link.
(setq links (list l0 l1))
(setq joint-list (list hinge-joint))
;; 5. call :init-ending after defining links and joint-list and return "self"
(send self :init-ending)
self)))
;; Methods to define robot links
(:make-root-link
()
(let ((b0 (make-cube 10 1330 1590))
(b1 (make-cube 130 200 250))
(b1- (make-cube 50 190 240))
(b2 (make-cylinder 30 40)))
(send b1- :translate (float-vector (+ (/ 130 -2.0) (/ 50 2.0) -5) 0 0))
(send b1- :worldcoords)
(setq b1 (body- b1 b1-))
(send b0 :translate (float-vector (/ 10 2.0) 0 (/ 1800 2.0)))
(send b1 :translate (float-vector (/ 130 -2.0) 0 (+ 1330 (/ 250 2.0))))
(send b2 :translate (float-vector -105 0 (+ 1330 (/ 250 2.0))) :world)
(send b2 :rotate -pi/2 :y)
(send b0 :set-color #f(0.8 0.5 0))
(send b1 :set-color :gray60)
(send b2 :set-color :red)
(send b0 :assoc b1)
(send b0 :assoc b2)
(instance bodyset-link :init (make-cascoords) :bodies (list b0 b1 b2))
))
(:make-door-link
()
(let ((b0 (make-cube 20 200 250)))
(send b0 :translate (float-vector (/ 20 -2.0) (/ 200 -2.0) (/ 250 2)))
(send b0 :set-color :gray60)
(instance bodyset-link :init (make-cascoords) :bodies (list b0))
))
)
(defclass drc-surprise-task-lever
:super cascaded-link
:slots (lever-joint)
)
(defmethod drc-surprise-task-lever
(:init
()
(prog1
(send-super :init :name "drc-surprise-task-lever")
;; 1. make links links and assoc all links
(let ((l0 (send self :make-root-link))
(l1 (send self :make-lever-link)))
(send l1 :translate (float-vector 0 (/ 405 -2.0) (+ 910 455)))
;; 2. assoc links
;; Root link should be associated with "self".
(send self :assoc l0)
(send l0 :assoc l1)
;; 3. make all joints
;; Before making joints, you should :assoc all links.
(setq lever-joint (instance rotational-joint :init
:parent-link l0 :child-link l1
:axis :y
:name :lever-joint
:min -90 :max 90))
;; 4. define slots for robot class
;; links and joint-list for cascaded-link.
(setq links (list l0 l1))
(setq joint-list (list lever-joint))
;; 5. call :init-ending after defining links and joint-list and return "self"
(send self :init-ending)
self)))
;; Methods to define robot links
(:make-root-link
()
(let ((b0 (make-cube 10 1330 1590))
(b1 (make-cube 150 405 720)))
(send b0 :translate (float-vector (/ 10 2.0) 0 (/ 1800 2.0)))
(send b1 :translate (float-vector (/ 150 -2.0) 0 (+ 910 (/ 720 2.0))))
(send b1 :set-color :gray50)
(send b0 :set-color #f(0.8 0.5 0))
(send b0 :assoc b1)
(instance bodyset-link :init (make-cascoords) :bodies (list b0 b1))
))
(:make-lever-link
()
(let ((b0 (make-cube 230 10 30))
(b1 (make-cylinder (/ 58 2.0) 10)))
(send b0 :translate (float-vector (/ 230 -2.0) (/ 10 -2.0) 0))
(send b1 :translate (float-vector -230 0 0))
(send b1 :rotate pi/2 :x)
(send b0 :set-color :gray60)
(send b1 :set-color :red)
(send b0 :assoc b1)
(instance bodyset-link :init (make-cascoords) :bodies (list b0 b1))
))
)
(defclass drc-surprise-task-rope
:super single-link-model
:slots ()
)
(defmethod drc-surprise-task-rope
(:init
()
(let ((blist (send self :make-surprise-task-rope-bodies)))
(dolist (b (cdr blist)) (send (car blist) :assoc b))
(send-super :init :name "drc-surprise-task-rope" :bodies blist)))
(:make-surprise-task-rope-bodies
()
(let ((b0 (make-cube 10 1330 1590))
(b1 (make-cube 50 100 50))
(b2 (make-cube 10 1040 10)))
(send b0 :translate (float-vector (/ 10 2.0) 0 (/ 1800 2.0)))
(send b1 :translate (float-vector (/ 50 -2.0) -480 1435))
(send b2 :translate (float-vector (/ 10 -2.0) 80 1435))
(send b0 :set-color #f(0.8 0.5 0))
(send b1 :set-color :blue)
(send b2 :set-color :red)
(send b0 :assoc b1)
(send b0 :assoc b2)
(list b0 b1 b2)))
)
;;;;;;;;;;;;;;;;
;; Model generation functions
;;;;;;;;;;;;;;;;
(defun make-drc-stair (&key (add-groud-p))
(setq *stair* (instance drc-stair :init :add-groud-p add-groud-p))
)
(defun make-drc-door ()
"Door on drc"
; (null-output (load "package://drc_task_common/euslisp/test-drc-door-task.l"))
(load "test-drc-door-task.l")
(setq *door* (instance param-door :init 900
60 102 20
(float-vector 0 (+ 900 -60 -24) 845)
(float-vector -60 (+ 900 -60 -24) 845)
:handle-l/r :left))
)
(defun make-drc-hose-wall ()
(setq *hose-wall* (instance drc-hose-wall :init))
)
(defun make-drc-hose-plug ()
(setq *hose-plug* (instance drc-hose-plug :init))
)
(defun make-drc-drill-wall ()
(setq *drill-wall* (instance drc-drill-wall :init))
)
(defun make-drc-terrain (&key (add-groud-p))
(setq *terrain* (instance drc-terrain :init :add-groud-p add-groud-p))
)
(defun make-drc-terrain-japanese-block-ver (&key (add-groud-p))
(setq *terrain* (instance drc-terrain :init :block-dimensions (list 390 190 150) :add-groud-p add-groud-p))
)
(defun make-drc-terrain-japanese-block-ver-sagami (&key (add-groud-p))
(setq *terrain* (instance drc-terrain :init :block-dimensions (list 390 190 150) :add-groud-p add-groud-p :block-angle 12.5))
)
(defun make-drc-valve ()
(setq *drc-valve* (instance valve :init :radius (/ 260 2.0) :thick (/ (- 260 205) 2.0) :ring-color :red))
)
(defun make-drc-surprise-task-shower ()
(setq *surprise-task-shower* (instance drc-surprise-task-shower :init))
)
(defun make-drc-surprise-task-button ()
(setq *surprise-task-button* (instance drc-surprise-task-button :init))
)
(defun make-drc-surprise-task-lever ()
(setq *surprise-task-lever* (instance drc-surprise-task-lever :init))
)
(defun make-drc-surprise-task-rope ()
(setq *surprise-task-rope* (instance drc-surprise-task-rope :init))
)
(defun make-drc-surprise-task-box ()
(make-drc-surprise-task-shower)
(make-drc-surprise-task-button)
(make-drc-surprise-task-lever)
(make-drc-surprise-task-rope)
(send *surprise-task-shower* :translate (float-vector (/ 1330 -2.0) 0 0))
(send *surprise-task-lever* :translate (float-vector (/ 1330 2.0) 0 0))
(send *surprise-task-lever* :rotate pi :z)
(send *surprise-task-button* :translate (float-vector 0 (/ 1330 -2.0) 0))
(send *surprise-task-button* :rotate pi/2 :z)
(send *surprise-task-rope* :translate (float-vector 0 (/ 1330 2.0) 0))
(send *surprise-task-rope* :rotate -pi/2 :z)
(list *surprise-task-shower* *surprise-task-button* *surprise-task-lever* *surprise-task-rope*)
)
;; TODO : This should be scene
(defun make-drc-testbed-models ()
;; Generate models
(make-drc-door)
(make-drc-hose-plug)
(make-drc-hose-wall)
(make-drc-stair)
(make-drc-drill-wall)
(make-drc-terrain)
(make-drc-valve)
(let ((sb (make-drc-surprise-task-box)))
;(send-all sb :translate (float-vector 12000 0 0) :world)
)
;; Align models
; (send *stair* :translate (float-vector 8000 0 0) :world)
; (send *door* :translate (float-vector 0 0 0) :world)
(send *hose-wall* :rotate (deg2rad -45) :z)
; (send *hose-wall* :translate (float-vector 4000 -3000 0) :world)
(send *drill-wall* :rotate -pi/2 :z)
(objects (list *drill-wall*))
; (send *drill-wall* :translate (float-vector 2000 -3500 0) :world)
; (send *terrain* :translate (float-vector 5000 -1000 0) :world)
(send *drc-valve* :rotate -pi/2 :y)
(send *drc-valve* :rotate (deg2rad -135) :x)
; (send *drc-valve* :translate (float-vector 2000 -3500 1130) :world)
; (send *drc-valve* :translate (float-vector 0 (/ 1210 -2.0) (+ -110 -400)))
(send *drc-valve* :translate (float-vector 0 0 (+ 1130 (+ -110 -400))))
;; Didplay
(setq *models* (list *hose-plug* *hose-wall* *stair* *door* *drill-wall* *terrain* *drc-valve*
*surprise-task-shower* *surprise-task-button* *surprise-task-lever* *surprise-task-rope*))
(objects *models*)
)
;(load "/home/k-okada/prog/euslib/rbrain/wrl2eus.l")
(progn ;(load "package://drc_task_common/euslisp/drc-testbed-models.l")
(make-drc-testbed-models)
(send *door* :name "drc-testbed-door")
;; (eus2wrl *door* "/tmp/DRCTestbedDoor.wrl" :mode :openhrp3 :fixed t )
(send *drill-wall* :name "drc-testbed-drill-wall")
;; (eus2wrl *drill-wall* "/tmp/DRCTestbedDrillWall.wrl" :mode :openhrp3 :fixed t)
(send *hose-plug* :name "drc-testbed-hose-plug")
;; (eus2wrl *hose-plug* "/tmp/DRCTestbedHosePlug.wrl" :mode :openhrp3 :fixed t)
(send *hose-wall* :name "drc-testbed-hose-wall")
;; (eus2wrl *hose-wall* "/tmp/DRCTestbedHoseWall.wrl" :mode :openhrp3 :fixed t)
(send *drc-valve* :name "drc-testbed-valve")
;; (eus2wrl *drc-valve* "/tmp/DRCTestbedValve.wrl" :mode :openhrp3 :fixed t)
(send *surprise-task-shower* :name "drc-testbed-shower")
;; (eus2wrl *surprise-task-shower* "/tmp/DRCTestbedShower.wrl" :mode :openhrp3 :fixed t)
(send *surprise-task-button* :name "drc-testbed-button")
;; (eus2wrl *surprise-task-button* "/tmp/DRCTestbedButton.wrl" :mode :openhrp3 :fixed t)
(send *surprise-task-lever* :name "drc-testbed-lever")
;; (eus2wrl *surprise-task-lever* "/tmp/DRCTestbedLever.wrl" :mode :openhrp3 :fixed t)
(send *surprise-task-rope* :name "drc-testbed-rope")
;; (eus2wrl *surprise-task-rope* "/tmp/DRCTestbedRope.wrl" :mode :openhrp3 :fixed t)
(dolist (obj *models*)
(convert-to-irtmodel obj :output-directory "models"))
)
Raw
test-drc-door-task.l
#|
(load "~/prog/euslib/irteus_proposals/motion-lib-proposal.l")
(defun setup-door-open (&key (draw t) (generate-robot t))
(when generate-robot
(load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l")
(hrp2jsknts-init)
(setq *robot* *hrp2jsknts*))
;;(make-drc-box2-door)
;;(make-drc-test-door-right)
(when draw (objects (list *robot*)))
;; initialize end-coords for door-grasp
(mapcar #'(lambda (arm trs)
(let ((mc (make-cascoords
:coords (send (send *robot* arm :end-coords :copy-worldcoords)
:translate trs)
:name (read-from-string (format nil "~A-tip-grasp-coords" arm)))))
(send *robot* :put (read-from-string (format nil "~A-tip-grasp-coords" arm)) mc)
(send (send (send *robot* arm :end-coords) :parent) :assoc mc)
))
'(:rarm :larm) (list #f(50 -25 0) #f(50 25 0)))
(defvar *rarm-avoid-pose* #f(40.1007 -29.388 4.91744 -71.6494 -22.7883 -2.70865 7.77037 15.0))
(defvar *larm-avoid-pose* #f(40.1007 29.388 -4.91744 -71.6494 22.7883 2.70865 7.77037 15.0))
;; (defvar *rarm-avoid-pose* #f(50.0 -10.0 0.0 -90.0 0.0 0.0 0.0 0.0))
;; (defvar *larm-avoid-pose* #f(50.0 10.0 0.0 -90.0 0.0 0.0 0.0 0.0))
(defvar *larm-reset-manip-pose* #f(50.0 30.0 10.0 -120.0 25.0 5.0 -20.0 -60.0))
(defvar *rarm-reset-manip-pose* #f(50.0 -30.0 -10.0 -120.0 -25.0 -5.0 -20.0 60.0))
(defvar *door-grasp-preshape-pose* #f(0.0 70.0 0.0 -10.0 30.0 30.0))
(defvar *door-grasp-pose* #f(0.0 -20.0 10.0 -10.0 120.0 120.0))
)
;;(warn ";; (setup-door-open)~%")
(defun setup-door-open-real (&key (start-abc-st t))
;; ABC, ST, IMP param
;; save original parameter of ABC and ST
(let* ((gait-generator-param (send (send *ri* :get-gait-generator-param) :slots))
(auto-balancer-param (send (send *ri* :get-auto-balancer-param) :slots)))
(setq *original-default-step-time*
(cdr (assoc 'ros::_default_step_time gait-generator-param)))
(setq *original-default-double-support-ratio*
(cdr (assoc 'ros::_default_double_support_ratio gait-generator-param)))
(setq *original-default-zmp-offsets*
(list (subseq (scale 1000 (send (cdr (assoc 'ros::_default_zmp_offsets auto-balancer-param)) :data)) 0 3)
(subseq (scale 1000 (send (cdr (assoc 'ros::_default_zmp_offsets auto-balancer-param)) :data)) 3 6)))
)
;; set new parameter
(send *ri* :set-gait-generator-param :default-step-time 0.9 :default-double-support-ratio 0.32)
(send *ri* :set-auto-balancer-param :default-zmp-offsets (list (float-vector 0 10 0) (float-vector 0 -10 0)))
;;(send *ri* :set-gait-generator-param :default-step-time 1.0 :default-double-support-ratio 0.35)
;;(send *ri* :set-auto-balancer-param :default-zmp-offsets (list (float-vector 0 -10 0) (float-vector 0 10 0)))
(send *ri* :set-impedance-controller-param :arms :k-p 200 :d-p 180 :m-p 5 :m-r 1 :d-r 15 :k-r 20)
(send *ri* :start-impedance :arms)
(when start-abc-st
(send *ri* :start-auto-balancer :limbs '(:rleg :lleg :rarm :larm))
(send *ri* :start-st))
;; (send *ri* :remove-force-sensor-offset)
;; (send *ri* :reset-force-moment-offset-arms)
)
;;(warn ";; (setup-door-open-real)~%")
(defun set-original-abc-st-param-from-door
()
(when (and (boundp '*original-default-step-time*) (boundp '*original-default-double-support-ratio*))
(send *ri* :set-gait-generator-param
:default-step-time *original-default-step-time*
:default-double-support-ratio *original-default-double-support-ratio*))
(when (boundp '*original-default-zmp-offsets*)
(send *ri* :set-auto-balancer-param
:default-zmp-offsets *original-default-zmp-offsets*))
)
|#
(defclass param-door
:super cascaded-link
:slots (handles)
)
(defmethod param-door
(:init
(door-width door-knob-depth door-knob-width door-knob-height
door-knob-pos door-handle-pos
&key (handle-l/r :left) (hinge-l/r (if (eq handle-l/r :left) :right :left)))
(send-super :init)
(let* ((door-depth 30) (door-height 1800)
(b0 (make-cube 1 1 1))
(b1 (make-cube door-depth door-width door-height))
(b2 (make-cube door-knob-depth door-knob-width door-knob-height))
(b-leg1 (make-cube 700 100 100))
(b-leg2 (make-cube 700 100 100)))
(send b-leg1 :set-color :gray)
(send b-leg2 :set-color :gray)
(send b1 :set-color :gray90)
(send b2 :set-color :gray60)
(send b1 :translate (float-vector 0 (/ door-width (case handle-l/r (:left 2.0) (t -2.0))) (/ door-height 2.0)))
(send b2 :locate (float-vector (/ door-knob-depth -2.0) (/ door-knob-width (case handle-l/r (:left -2.0) (t 2.0))) 0))
(send b-leg1 :translate (float-vector -350 (* (case handle-l/r (:right 1.0) (t -1.0)) 50) 50))
(send b-leg2 :translate (float-vector -350 (* (case handle-l/r (:right 1.0) (t -1.0)) (- -50 door-width)) 50))
(mapcar #'(lambda (pb cb)
(send pb :assoc cb))
(list b0 b1) (list b1 b2))
(let* ((l0 (instance bodyset-link :init (make-cascoords) :bodies (list b-leg1 b-leg2) :name :root-link))
(l1 (instance bodyset-link :init (make-cascoords) :bodies (list b1) :name :door-panel))
(l2 (instance bodyset-link :init (make-cascoords) :bodies (list b2) :name :door-knob)))
(send l2 :locate door-knob-pos :world)
(let ((j0 (instance rotational-joint :init :name :door-hinge-joint
:parent-link l0 :child-link l1 :axis (case hinge-l/r (:left :z) (t :-z))))
(j1 (instance rotational-joint :init :name :door-knob-joint
:parent-link l1 :child-link l2 :axis (case handle-l/r (:left :-x) (t :x))))
(h0 (make-cascoords :pos door-handle-pos :rpy (list 0 (deg2rad 0) pi/2) :name :larm-knob-handle))
(h1 (make-cascoords :pos door-handle-pos :rpy (list 0 (deg2rad 0) -pi/2) :name :rarm-knob-handle)))
(mapcar #'(lambda (pl cl)
(send pl :assoc cl))
(list self l0 l1 l2 l2) (list l0 l1 l2 h0 h1))
(setq links (list l0 l1 l2))
(setq joint-list (list j0 j1))
(setq handles (list h0 h1))
(send self :init-ending)
self))))
(:handle (&optional name) (if name
(find name handles :test #'equal :key #'(lambda (x) (send x :name)))
handles))
)
#|
;; drc box2 door param
;; door width : 700mm
;; hinge -> knob-axis : 640mm
;; knob depth : 60mm
;; knob width : 100mm
;; knob height : 20mm
;; knob pos z : 950mm
(defun make-drc-box2-door ()
"Door on drc box2."
(setq *door* (instance param-door :init 700 60 100 20
(float-vector 0 -640 950)
(float-vector -60 -600 950)
:handle-l/r :right)))
;; drc box2 door param
;; door width : 840mm
;; hinge -> knob-axis : 800mm
;; knob depth : 60mm
;; knob width : 120mm
;; knob height : 20mm
;; knob pos z : 940mm
(defun make-drc-test-door-right ()
"DRC testing door (right-handled side)"
(setq *door* (instance param-door :init 840 60 120 20
(float-vector 0 -800 940)
(float-vector -60 -740 940)
:handle-l/r :right)))
(defun make-drc-test-door-left ()
"DRC testing door (left-handled side)"
(setq *door* (instance param-door :init 840 60 120 20
(float-vector 0 800 940)
(float-vector -60 740 940)
:handle-l/r :left)))
(defun make-drc-sagami-door ()
"drc door in sagami full-task-course environment"
(setq *door* (instance param-door :init 895 60 110 20
(float-vector 0 800 845)
(float-vector -50 760 845)
;; (float-vector 0 800 1120)
;; (float-vector -50 760 1120)
:handle-l/r :left))
)
(defun exec-robot-state-list
(rs-list &key (real nil))
"Execute robot state list.
rs-list is robot state list, e.g., list of (list :angle-vector av :time tm :root-coords :hand-angle-vector hav ...)"
(dolist (rs rs-list)
(send *robot* :angle-vector (cadr (memq :angle-vector rs)))
(send *robot* :hand-angle-vector (cadr (memq :hand-angle-vector rs)))
(send *robot* :move-coords (cadr (memq :root-coords rs)) (car (send *robot* :links)))
(when (boundp '*irtviewer*) (send *irtviewer* :draw-objects))
(if real
(bench
(send *ri* :hand-angle-vector (send *robot* :hand-angle-vector) (cadr (memq :time rs)))
(send *ri* :angle-vector (send *robot* :angle-vector) (cadr (memq :time rs)))
(send *ri* :wait-interpolation))
;;(read-line)
)
)
)
(defun get-current-robot-state
(tm)
"tm is angle-vector time [s]."
(list :angle-vector (send *robot* :angle-vector)
:hand-angle-vector (send *robot* :hand-angle-vector)
:root-coords (send (car (send *robot* :links)) :copy-worldcoords)
:time tm)
)
(defun fullbody-ik-for-door-open
(target-coords
&rest args &key (arm :larm)
&allow-other-keys)
(with-move-target-link-list
(mt ll *robot* (list arm :rleg :lleg))
(send* *robot* :fullbody-inverse-kinematics
(list target-coords
(send *robot* :rleg :end-coords :copy-worldcoords)
(send *robot* :lleg :end-coords :copy-worldcoords))
:additional-nspace-list
(list (list (send *robot* :torso :waist-p :child-link)
#'(lambda () (* 0.5 (elt (v* (send (send *robot* :torso :waist-p :child-link) :rotate-vector #F(0 0 1)) #f(0 0 1)) 1))))
(list (send *robot* :rarm :wrist-p :child-link)
#'(lambda () (* 0.05 (deg2rad (- 0.0 (send *robot* :rarm :wrist-p :joint-angle))))))
(list (send *robot* :larm :wrist-p :child-link)
#'(lambda () (* 0.05 (deg2rad (- 0.0 (send *robot* :larm :wrist-p :joint-angle)))))))
:move-target (append (list (send *robot* :get (read-from-string (format nil "~A-tip-grasp-coords" arm)))) (cdr mt))
:link-list ll
:look-at-target t
:debug-view :no-message
args))
)
(defun initialize-pose-for-door-open
(&key (real t)
(arm :larm)
(initial-standing-coords
(make-coords :pos (float-vector -650 -450 0)
:rpy (list (deg2rad -15) 0 0)))
(initial-waist-y 0))
(let ((rs-list))
;; initialize
(send (send *door* :joint :door-hinge-joint) :joint-angle 0)
(send (send *door* :joint :door-knob-joint) :joint-angle 0)
(send *robot* :reset-manip-pose)
(send *robot* :torso :waist-y :joint-angle initial-waist-y)
(send *robot* :fix-leg-to-coords (make-coords))
(send *robot* arm :move-end-rot (case arm (:larm 90) (t -90)) :x)
(send *robot* arm :move-end-rot (case arm (:larm 30) (t -30)) :z)
(send *robot* arm :move-end-pos (float-vector 25 0 50) :world)
(send *robot* :fix-leg-to-coords (send (send *door* :copy-worldcoords) :transform initial-standing-coords :local))
(case arm
(:larm (send *robot* :rarm :angle-vector *rarm-avoid-pose*))
(:rarm (send *robot* :larm :angle-vector *larm-avoid-pose*)))
(send (send *robot* :hand arm) :angle-vector *door-grasp-preshape-pose*)
(when (boundp '*irtviewer*) (objects (list *robot* *door*)))
(exec-robot-state-list (list (get-current-robot-state 5000)) :real real)
))
(defun reach-grasp-door
(&key (real t)
(arm :larm)
(way-point-num 2))
(let ((rs-list))
;; reaching poses
(let* ((knob-target-coords (send *door* :handle (read-from-string (format nil "~A-knob-handle" arm))))
(cds-list (list (send (send knob-target-coords :copy-worldcoords)
:translate (case arm (:larm (float-vector -100 -200 150)) (t (float-vector -100 0 150))) :world)
(send (send (send knob-target-coords :copy-worldcoords)
;;:translate (case arm (:larm (float-vector -50 50 150)) (t (float-vector -50 -50 150))) :world)
:translate (case arm (:larm (float-vector -50 -100 100)) (t (float-vector -50 0 150))) :world)
:rotate (case arm (:larm (deg2rad 10)) (t (deg2rad -10))) :z) ;; way point2
knob-target-coords ;; target point
)))
(if (= way-point-num 1) (setq cds-list (cdr cds-list)))
(dolist (cds cds-list)
(fullbody-ik-for-door-open cds :arm arm :rotation-axis (list (if (= (position cds cds-list) 0) :z t) t t))
(when (boundp '*irtviewer*) (send *irtviewer* :draw-objects))
(push (get-current-robot-state 2000) rs-list)
))
;; grasp
(send (send *robot* :hand arm) :angle-vector *door-grasp-pose*)
(push (get-current-robot-state 2000) rs-list)
(reverse rs-list)
))
(defun release-grasp-door
(&key (real t)
(arm :larm)
(initial-standing-coords
(make-coords :pos (float-vector -650 -450 0)
:rpy (list (deg2rad -15) 0 0)))
(release-way-point (case arm
(:larm (float-vector -50 -100 100))
(t (float-vector -50 100 100))))
(initialize-p t))
(let ((rs-list))
;; initialize
(send (send *robot* :hand arm) :angle-vector *door-grasp-preshape-pose*)
(push (get-current-robot-state 2000) rs-list)
;; reaching poses
(let* ((current-coords (send *robot* :get (read-from-string (format nil "~A-tip-grasp-coords" arm))))
(cds-list (list (send (send (send current-coords :copy-worldcoords)
:translate release-way-point :world)
:rotate (case arm (:larm (deg2rad 10)) (t (deg2rad -10))) :z)))) ;; way point 1
(dolist (cds cds-list)
(fullbody-ik-for-door-open cds :arm arm)
(when (boundp '*irtviewer*) (send *irtviewer* :draw-objects))
(push (get-current-robot-state 2000) rs-list)
))
;; grasp
(if initialize-p (initialize-pose-for-door-open :real nil :arm arm :initial-standing-coords initial-standing-coords))
(push (get-current-robot-state 2000) rs-list)
(reverse rs-list)
))
(defun pull-push-door-by-arm
(&rest args
&key (arm :larm)
(push/pull :pull)
(initial-standing-coords
(make-coords :pos (float-vector -650 -450 0)
:rpy (list (deg2rad -15) 0 0)))
(angle-list (list 0 -10 -15))
&allow-other-keys)
(let ((rs-list))
(block
:ik-loop
(dolist (door-ja angle-list)
(send (send *door* :joint :door-hinge-joint) :joint-angle (case push/pull (:pull door-ja) (t (- door-ja))))
;; rotate knob
(send (send *door* :joint :door-knob-joint) :joint-angle
(if (or (= (position door-ja angle-list) 0) (= (position door-ja angle-list) 1)) -55.0 0.0))
(let ((ik-ret
(apply #'fullbody-ik-for-door-open
(send *door* :handle (read-from-string (format nil "~A-knob-handle" arm)))
:arm arm args)))
(cond
((or (null ik-ret) (send *robot* :self-collision-check))
(return-from :ik-loop nil))
(t
(push (get-current-robot-state 2000) rs-list)))
)))
(reverse rs-list)
))
(defmacro with-preserve-robot-state
(robot &rest args)
"Macro with preserving robot state (angle-vector, worldcoords).
args is arbitral s-expression.
Even if robot state is changed in args function, this macro return to the robot state before execution of args."
(with-gensyms
(prev-av prev-rc)
`(let ((,prev-av (send ,robot :angle-vector))
(,prev-rc (send ,robot :copy-worldcoords)))
(prog1
,@args
;; return to original state
(send ,robot :angle-vector ,prev-av)
(send ,robot :newcoords ,prev-rc)
))))
(defun get-actual-robot-state-with-rpy-estimation
()
"Reflect actual robot state (potentio-vector and estimated rpy).
Robot foot pos and yaw rotation is fixed."
(let ((ref-yaw (caar (send (send *robot* :foot-midcoords) :rpy-angle)))
(ref-fm-pos (send (send *robot* :foot-midcoords) :worldpos)))
(send *ri* :state)
(send *robot* :angle-vector (send *ri* :potentio-vector))
(let* ((act-fm-ypr (car (send (send (send *ri* :robot) :foot-midcoords) :rpy-angle))))
(send *robot* :fix-leg-to-coords
(make-coords :pos ref-fm-pos
:rpy (list ref-yaw (elt act-fm-ypr 1) (elt act-fm-ypr 2))))
(when (boundp '*irtviewer*) (send *irtviewer* :draw-objects))
)))
(defun estimate-hand-diff-pos
(&key (arm :larm) (func))
"Estimate hand difference betweeen before execution of func and after execution of func."
(let ((pos1) (pos2))
(with-preserve-robot-state
*robot*
(get-actual-robot-state-with-rpy-estimation)
(setq pos1 (copy-object (send (send *robot* :get (read-from-string (format nil "~A-tip-grasp-coords" arm))) :worldpos))))
(with-preserve-robot-state
*robot*
(funcall func)
(get-actual-robot-state-with-rpy-estimation)
(setq pos2 (copy-object (send (send *robot* :get (read-from-string (format nil "~A-tip-grasp-coords" arm))) :worldpos))))
(v- pos2 pos1)))
(defun door-try-push-pull
(&key (arm :larm) (push/pull :push) (push/pull-stride 40))
(estimate-hand-diff-pos
:arm arm
:func
#'(lambda ()
;; try push or pull
(fullbody-ik-for-door-open
(send (send (send *robot* :get (read-from-string (format nil "~A-tip-grasp-coords" arm))) :copy-worldcoords)
:translate (float-vector (* (case push/pull (:push 1) (t -1)) push/pull-stride) 0 0))
:arm arm)
(exec-robot-state-list
(list (get-current-robot-state 1500)) :real t)
(unix:sleep 1))))
(defun door-push-pull-detection
(&key (arm :larm) (knob-ja -55))
"Detect push or pull.
Return value is :push, :pull, or nil.
nil is too small hand diff for both push and pull."
(let ((push-diff) (pull-diff))
(send (send *door* :joint :door-knob-joint) :joint-angle knob-ja)
(fullbody-ik-for-door-open
(send *door* :handle (read-from-string (format nil "~A-knob-handle" arm)))
:arm arm)
(exec-robot-state-list
(list (get-current-robot-state 1000)) :real t)
(with-preserve-robot-state
*robot*
(setq push-diff (door-try-push-pull :arm arm :push/pull :push)))
(with-preserve-robot-state
*robot*
(setq pull-diff (door-try-push-pull :arm arm :push/pull :pull)))
(let ((ret
(cond
((and (< (norm pull-diff) 10.0) (< (norm push-diff) 10.0)) ;; [mm]
nil)
((< (norm pull-diff) (norm push-diff))
:push)
(t :pull))))
(format t ";; push-diff-norm ~A, pull-diff-norm ~A => ~A~%"
(norm push-diff) (norm pull-diff)
(if ret ret "too small diff!! Failed to open?"))
;; return to initial pose
(exec-robot-state-list
(list (get-current-robot-state 1000)) :real t)
ret)))
(defun test-door-open-drcbox2-door (&key (walk-p t) (wait t) (draw t))
"Test for drc box2 door"
(print ";; Initial pose")
(make-drc-box2-door)
(when draw (objects (list *robot* *door*)))
;; (send *door* :move-coords
;; (send (send (send (send (car (send *robot* :links)) :copy-worldcoords) :transform (make-coords :pos (float-vector 642.373 43.578 296.552) :rpy (list -1.343 -0.009 -1.602))) :rotate (deg2rad 90) :y) :rotate (deg2rad 180) :z)
;; (send *door* :handle :larm-knob-handle))
;; (send *door* :translate #f(20 -50 0) :world)
(setq *init-standing-coords* (make-coords :pos (float-vector -650 -450 0) :rpy (list (deg2rad -15) 0 0)))
(initialize-pose-for-door-open :real nil :arm :larm :initial-standing-coords *init-standing-coords* :initial-waist-y 0)
(exec-robot-state-list (list (get-current-robot-state 3000)) :real t)
(print ";; Reaching and open. Press Enter")
(when wait (read-line))
(setq *rs-list* (reach-grasp-door :real nil))
(exec-robot-state-list *rs-list* :real t)
(let ((push/pull (door-push-pull-detection :arm :larm)))
(when push/pull
(setq *rs-list* (pull-push-door-by-arm :push/pull push/pull))
(exec-robot-state-list *rs-list* :real t)
(when walk-p
(print ";; go-velocity Press Enter")
(when wait (read-line))
(let ((rc (send (send *robot* :foot-midcoords) :transformation (send *robot* :larm :end-coords))))
(send *ri* :set-auto-balancer-param :graspless-manip-arm "larm"
:graspless-manip-mode t
;;:graspless-manip-p-gain #f(0.9 0.9 0.9)
:graspless-manip-p-gain #f(0.7 0.7 0.9)
:graspless-manip-reference-trans-pos (send rc :worldpos)
:graspless-manip-reference-trans-rot (send rc :worldrot)))
(send* *ri* :go-velocity (send *ri* :calc-go-velocity-param-from-velocity-center-offset -6.0 (send (send (send *robot* :foot-midcoords) :transformation (send (send *door* :joint :door-hinge-joint) :child-link)) :worldpos)))
;;(send* *ri* :go-velocity (send *ri* :calc-go-velocity-param-from-velocity-center-offset -4.5 (send (send (send *robot* :foot-midcoords) :transformation (send (send *door* :joint :door-hinge-joint) :child-link)) :worldpos)))
;;(unix:sleep 3)
;;(unix:sleep 5)
;;(unix:sleep 18)
(unix:sleep 10)
;;(unix:sleep 5)
(send *ri* :go-stop)
)
))
(print ";; release")
(when wait (read-line))
(setq *rs-list* (release-grasp-door :real nil :arm :larm :initial-standing-coords *init-standing-coords*))
(exec-robot-state-list *rs-list* :real t)
)
(defun test-door-open-drc-test-door-right (&key (walk-p t))
"Test for drc-test door right"
(make-drc-test-door-right)
(objects (list *robot* *door*))
(setq *init-standing-coords* (make-coords :pos (float-vector -600 -400 0) :rpy (list (deg2rad 20) 0 0)))
(initialize-pose-for-door-open :real nil :arm :rarm :initial-standing-coords *init-standing-coords* :initial-waist-y -20)
(send *robot* :head :look-at (send (send (send *door* :joint :door-knob-joint) :child-link) :worldpos))
(exec-robot-state-list (list (get-current-robot-state 3000)) :real t)
(print ";; reach and open")
(read-line)
(setq *rs-list* (reach-grasp-door :real nil :arm :rarm))
(exec-robot-state-list *rs-list* :real t)
(let ((push/pull (door-push-pull-detection :arm :rarm)))
(when push/pull
;;(read-line)
(setq *rs-list* (pull-push-door-by-arm :arm :rarm :initial-standing-coords *init-standing-coords* :push/pull push/pull :angle-list (list 0 -5 -10)))
(exec-robot-state-list *rs-list* :real t))
;;(read-line)
(setq *rs-list* (release-grasp-door :real nil :arm :rarm :initial-standing-coords *init-standing-coords*))
(exec-robot-state-list *rs-list* :real t)
(send (send *robot* :hand :rarm) :angle-vector *door-grasp-pose*)
(exec-robot-state-list (list (get-current-robot-state 1000)) :real t))
(when walk-p
(print ";; go-pos fwd")
(read-line)
(dotimes (i 2)
(let* ((fwd-param (float-vector 50 0 0))
(go-pos-param (send (send *robot* :foot-midcoords) :inverse-rotate-vector fwd-param)))
(send *ri* :go-pos (* 1e-3 (elt go-pos-param 0)) (* 1e-3 (elt go-pos-param 1)) 0)
(send *robot* :translate fwd-param)))
(print ";; move door by rarm")
(read-line)
(initialize-pose-for-door-open :real nil :arm :rarm :initial-standing-coords *init-standing-coords* :initial-waist-y 0)
(send *robot* :rarm :angle-vector *rarm-reset-manip-pose*)
(send *robot* :rarm :move-end-pos #f(300 100 0) :world :rotation-axis nil)
(send (send *robot* :hand :rarm) :angle-vector *door-grasp-pose*)
(exec-robot-state-list (list (get-current-robot-state 2000)) :real t)
(initialize-pose-for-door-open :real nil :arm :rarm :initial-standing-coords *init-standing-coords* :initial-waist-y 0)
(send *robot* :rarm :angle-vector *rarm-reset-manip-pose*)
(send (send *robot* :hand :rarm) :angle-vector *door-grasp-pose*)
(exec-robot-state-list (list (get-current-robot-state 1000)) :real t)
(print ";; go-pos rotate")
(read-line)
;;(send *ri* :go-pos 0 0 70)
(dotimes (i 3)
(send *ri* :go-pos 0 0 20))
;; (send *ri* :go-pos 0 0 20)
;; (send *ri* :go-pos 0 -0.5 0)
;; (send *ri* :go-pos 0 -0.5 0)
)
)
(defun test-door-open-drc-test-door-left (&key (walk-p t))
"Test for drc-test door left"
(print ";; Initial pose")
(make-drc-test-door-left)
(objects (list *robot* *door*))
(setq *init-standing-coords* (make-coords :pos (float-vector -750 650 0) :rpy (list (deg2rad -10) 0 0)))
(initialize-pose-for-door-open :real nil :arm :rarm :initial-standing-coords *init-standing-coords* :initial-waist-y 20)
(send *robot* :head :look-at (send (send (send *door* :joint :door-knob-joint) :child-link) :worldpos))
(exec-robot-state-list (list (get-current-robot-state 3000)) :real t)
(print ";; Reach and open. Press enter")
(read-line)
(setq *rs-list* (reach-grasp-door :real nil :arm :rarm))
(exec-robot-state-list *rs-list* :real t)
(let ((push/pull (door-push-pull-detection :arm :rarm)))
(when push/pull
(setq *rs-list* (pull-push-door-by-arm :arm :rarm :initial-standing-coords *init-standing-coords*
:push/pull push/pull :angle-list (list 0 -5 -10 -25 -30)))
(exec-robot-state-list *rs-list* :real t))
(print ";; Release door.")
(setq *rs-list* (release-grasp-door :real nil :arm :rarm :initial-standing-coords *init-standing-coords*
:release-way-point (float-vector -55 -100 100))))
(send (send *door* :joint :door-hinge-joint) :joint-angle 30)
(exec-robot-state-list *rs-list* :real t)
(send *robot* :hand :arms :angle-vector *door-grasp-pose*)
(send *robot* :larm :angle-vector *larm-reset-manip-pose*)
(send *robot* :rarm :angle-vector *rarm-avoid-pose*)
(send *robot* :move-centroid-on-foot :both '(:rleg :lleg))
(exec-robot-state-list (list (get-current-robot-state 2000)) :real t)
(when walk-p
(print ";; go-pos bwd. Press Enter.")
(read-line)
(let* ((bwd-param (float-vector -250 0 0))
(go-pos-param (send (send *robot* :foot-midcoords) :inverse-rotate-vector bwd-param)))
(send *ri* :go-pos (* 1e-3 (elt go-pos-param 0)) (* 1e-3 (elt go-pos-param 1)) 0))
(print ";; Reach larm. Press Enter.")
(read-line)
(send *robot* :translate #f(-250 0 0) :world)
(fullbody-ik-for-door-open
(send (send (send *door* :handle :larm-knob-handle) :copy-worldcoords) :translate (float-vector 150 0 -150))
:translation-axis (list t t t)
:arm :larm
:rotation-axis (list nil t t))
(exec-robot-state-list (list (get-current-robot-state 1000)) :real t)
(fullbody-ik-for-door-open
(send (send (send *door* :handle :larm-knob-handle) :copy-worldcoords) :translate (float-vector 150 0 0))
:translation-axis (list t t t)
:arm :larm
:rotation-axis (list nil t t))
(exec-robot-state-list (list (get-current-robot-state 1000)) :real t)
(setq *rs-list* (pull-push-door-by-arm :arm :larm :initial-standing-coords *init-standing-coords* :push/pull :push :angle-list (list -35 -45 -55) :rotation-axis (list nil t t)))
(exec-robot-state-list *rs-list* :real t)
(print ";; gopos -30 and reset-manip-pose")
(read-line)
(send *ri* :go-pos 0 0 -30)
(send *robot* :reset-manip-pose)
(send *robot* :arms :move-end-pos #f(70 0 0))
(send *robot* :move-centroid-on-foot :both '(:rleg :lleg))
(exec-robot-state-list (list (get-current-robot-state 3000)) :real t)
(dotimes (i 2)
(send *ri* :go-pos 0 0 -30))
(read-line)
(send *ri* :go-pos 0.15 0 0)
)
)
(defun test-door-open-drc-sagami-door (&key (walk-p t) (wait t) (draw t) (real t))
"drc door in sagami full-task-course environment"
(make-drc-sagami-door)
(when draw (objects (list *robot* *door*)))
(setq *init-standing-coords* (make-coords :pos (float-vector -600 400 0) :rpy (list (deg2rad -20) 0 0)))
;;(setq *init-standing-coords* (make-coords :pos (float-vector -750 650 0) :rpy (list (deg2rad -10) 0 0)))
;; initial pose
(initialize-pose-for-door-open :real nil :arm :larm :initial-standing-coords *init-standing-coords* :initial-waist-y -20)
(send *robot* :head :look-at (send (send (send *door* :joint :door-knob-joint) :child-link) :worldpos))
(exec-robot-state-list (list (get-current-robot-state 3000)) :real real)
(print ";; reach and open")
(when wait (read-line))
;; reach
(setq *rs-list* (reach-grasp-door :real nil :arm :larm))
(exec-robot-state-list *rs-list* :real real)
;; push
(let ((push/pull :push))
(when push/pull
(setq *rs-list* (pull-push-door-by-arm :arm :larm :initial-standing-coords *init-standing-coords* :push/pull push/pull :angle-list (list 0 -5 -10 -15)))
(exec-robot-state-list *rs-list* :real real))
(setq *rs-list* (release-grasp-door :real nil :arm :larm :initial-standing-coords *init-standing-coords*))
(exec-robot-state-list *rs-list* :real real)
(send (send *robot* :hand :larm) :angle-vector *door-grasp-pose*)
(exec-robot-state-list (list (get-current-robot-state 1000)) :real real))
;; walk
(when walk-p
(print ";; walk")
(when wait (read-line))
(let* ((walk-pos (float-vector 100 0 0)))
(setq walk-pos (send (send *door* :copy-worldcoords) :rotate-vector walk-pos))
(when real (send *ri* :go-pos (* 1e-3 (elt walk-pos 0)) (* 1e-3 (elt walk-pos 1)) 0))
(send *robot* :translate walk-pos :local)))
;; punch
(print ";; punch")
(when wait (read-line))
(initialize-pose-for-door-open :real nil :arm :larm
:initial-standing-coords *init-standing-coords* :initial-waist-y 0)
(send *robot* :larm :angle-vector *larm-reset-manip-pose*)
(send *robot* :larm :move-end-pos #f(200 -100 0) :local :rotation-axis nil)
(send (send *robot* :hand :larm) :angle-vector *door-grasp-pose*)
(exec-robot-state-list (list (get-current-robot-state 3000)) :real real)
(initialize-pose-for-door-open :real nil :arm :larm
:initial-standing-coords *init-standing-coords* :initial-waist-y 0)
(send *robot* :larm :angle-vector *larm-reset-manip-pose*)
(send *robot* :larm :move-end-pos #f(350 -100 0) :local :rotation-axis nil)
(send (send *robot* :hand :larm) :angle-vector *door-grasp-pose*)
(exec-robot-state-list (list (get-current-robot-state 1000)) :real real)
(initialize-pose-for-door-open :real nil :arm :larm
:initial-standing-coords *init-standing-coords* :initial-waist-y 0)
(send *robot* :larm :angle-vector *larm-reset-manip-pose*)
(send (send *robot* :hand :larm) :angle-vector *door-grasp-pose*)
(exec-robot-state-list (list (get-current-robot-state 3000)) :real real)
(send *robot* :larm :angle-vector *larm-avoid-pose*)
(send *robot* :rarm :angle-vector *rarm-avoid-pose*)
(exec-robot-state-list (list (get-current-robot-state 3000)) :real real)
)
#|
(defun door-initial-open
(&key (arm :larm))
(dolist (knob-ja (list -20 -40 -60))
(door-push-pull-detection :arm arm :knob-ja knob-ja)
(read-line)))
(defun test-check-walk ()
(send *ri* :go-velocity 0 0 0)
(send *robot* :torso :waist-y :joint-angle 40)
(send *ri* :angle-vector (send *robot* :angle-vector) 5000)
(send *ri* :wait-interpolation)
(send *robot* :torso :waist-y :joint-angle 0)
(send *ri* :angle-vector (send *robot* :angle-vector) 5000)
(send *ri* :wait-interpolation)
(send *ri* :go-stop)
)
(defun test-door-open-2 ()
(setq *init-standing-coords* (make-coords :pos (float-vector -600 -400 0) :rpy (list (deg2rad 20) 0 0)))
(initialize-pose-for-door-open :real nil :arm :rarm :initial-standing-coords *init-standing-coords* :initial-waist-y -20)
(send *robot* :head :look-at (send (send (send *door* :joint :door-knob-joint) :child-link) :worldpos))
(exec-robot-state-list (list (get-current-robot-state 3000)) :real t)
(setq *rs-list* (reach-grasp-door :real nil :arm :rarm))
(exec-robot-state-list *rs-list* :real t)
(read-line)
;;(setq *rs-list* (pull-push-door-by-arm :arm :rarm :initial-standing-coords (make-coords :pos (float-vector -550 -250 0)) :push/pull :push))
(setq *rs-list* (pull-push-door-by-arm :arm :rarm :initial-standing-coords (make-coords :pos (float-vector -550 -250 0)) :push/pull :push :angle-list (list 0 -5 -10)))
(exec-robot-state-list *rs-list* :real t)
(read-line)
(let ((rc (send (send *robot* :foot-midcoords) :transformation (send *robot* :rarm :end-coords))))
(send *ri* :set-auto-balancer-param :graspless-manip-arm "rarm"
:graspless-manip-mode t
:graspless-manip-p-gain #f(0.7 0.7 0.7)
:graspless-manip-reference-trans-pos (send rc :worldpos)
:graspless-manip-reference-trans-rot (send rc :worldrot)))
(send* *ri* :go-velocity (send *ri* :calc-go-velocity-param-from-velocity-center-offset 6.0 (send (send (send *robot* :foot-midcoords) :transformation (send (send *door* :joint :door-hinge-joint) :child-link)) :worldpos)))
(unix:sleep 1)
;;(unix:sleep 3)
;;(unix:sleep 5)
;;(unix:sleep 18)
(send *ri* :go-stop)
(read-line)
(exec-robot-state-list (reverse *rs-list*) :real t)
(read-line)
(release-grasp-door :real t :arm :rarm :initial-standing-coords (make-coords :pos (float-vector -550 -250 0) :rpy (list (deg2rad 15) 0 0)))
)
;; (dolist (rs ret) (send *robot* :angle-vector (cadr (memq :angle-vector rs))) (send *robot* :move-coords (cadr (memq :root-coords rs)) (car (send *robot* :links))) (send *irtviewer* :draw-objects) (read-line))
(defun test-door-open-3 ()
(make-drc-test-door-right)
(objects (list *robot* *door*))
(setq *init-standing-coords* (make-coords :pos (float-vector -600 -450 0) :rpy (list (deg2rad -40) 0 0)))
(initialize-pose-for-door-open :real nil :arm :larm :initial-standing-coords *init-standing-coords* :initial-waist-y 30)
;;(exec-robot-state-list (list (get-current-robot-state 3000)) :real t)
(send (send *robot* :hand :larm) :angle-vector #f(0.0 70.0 0.0 -10.0 30.0 30.0))
(setq *rs-list* (reach-grasp-door :real nil :arm :larm))
(exec-robot-state-list (list (elt *rs-list* 1)) :real nil)
(send *robot* :larm :move-end-pos #f(0 200 0) :world :rotation-axis :z)
(send *robot* :move-centroid-on-foot :both '(:rleg :lleg))
(exec-robot-state-list (list (get-current-robot-state 3000)) :real t)
(read-line)
(let ((go-pos-param (send (send *robot* :foot-midcoords) :inverse-rotate-vector (float-vector 50 0 0))))
(send *ri* :go-pos (* 1e-3 (elt go-pos-param 0)) (* 1e-3 (elt go-pos-param 1)) 0))
(read-line)
(exec-robot-state-list (cdr *rs-list*) :real t)
(read-line)
(setq *rs-list* (pull-push-door-by-arm :arm :larm :initial-standing-coords *init-standing-coords*
:push/pull :push :angle-list (list 0 -5 -10)))
(exec-robot-state-list *rs-list* :real t)
(read-line)
(setq *rs-list* (release-grasp-door :real nil :arm :larm :initial-standing-coords *init-standing-coords*))
)
|#
|#
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment