Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
- ;; util
- ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
- (defun make-default-robot-link
- (len
- radius
- axis
- name
- &key
- (extbody)
- (visualize-cylinder? t)
- )
- (let (bs b0 b1 c a (2r (* radius 2)))
- (setq b0 (make-cylinder (* 1.4 radius) (* 4 radius)))
- (setq b1 (make-cube 2r 2r len))
- (setq c (make-cascoords))
- (case axis
- (:x (setq a #f(1 0 0)))
- (:y (setq a #f(0 1 0)))
- (:z (setq a #f(0 0 1)))
- (:-x (setq a #f(-1 0 0)))
- (:-y (setq a #f(0 -1 0)))
- (:-z (setq a #f(0 0 -1)))
- (t (setq a axis)))
- (if (> (norm (v* a #f(0 0 -1))) 0)
- (send c :orient (acos (v. a #f(0 0 -1))) (v* a #f(0 0 -1)) :world))
- (when visualize-cylinder?
- (send b0 :transform c)
- (send b0 :translate (float-vector 0 0 (- 2r)))
- (send b0 :set-color :red)
- )
- (send b1 :translate (float-vector 0 0 (/ len -2)) :locate)
- (send b1 :set-color :green)
- (setq bs (append (if visualize-cylinder? (list b0 b1) (list b1)) extbody))
- (dolist (b (cdr bs))
- (send (car bs) :assoc b))
- (send-all bs :worldcoords) ;; for update centroid
- ;; set a mass center of default-robot-link as a volume center
- (let* ((valid-bodies
- (remove-if #'(lambda (x)
- (and (> (send x :volume) 0) (< (send x :volume) 0))) ;; nan check
- bs))
- (bodies-centroid
- (cond ((= (length valid-bodies) 0)
- (float-vector 0 0 0)
- )
- ((= (length valid-bodies) 1)
- (send (car valid-bodies) :centroid)
- )
- (t
- (scale (/ 1.0 (reduce #'+ (mapcar #'(lambda (x) (send x :volume)) valid-bodies)))
- (reduce #'v+ (mapcar #'(lambda (x) (scale (send x :volume) (send x :centroid))) valid-bodies)))
- )))
- )
- (instance bodyset-link :init (make-cascoords)
- :bodies bs :name name :centroid bodies-centroid)
- )))
- ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
- ;; two-stretchable-links-limb
- ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
- (defclass two-stretchable-links-limb
- :super robot-model
- :slots (root-coords
- middle-coords
- end-coords
- superior-link-length
- inferior-link-length
- joint-superior-0
- joint-superior-1
- joint-superior-2
- joint-linear-superior
- joint-middle
- joint-linear-inferior
- joint-inferior-0
- joint-inferior-1
- joint-inferior-2
- link-radius
- l-min-max-x-y
- )
- )
- (defmethod two-stretchable-links-limb
- (:init
- (&key
- (limb-name "default-limb")
- ((:link-radius tmp-link-radius) 12.5)
- ((:superior-link-length tmp-superior-link-length) 200.0)
- ((:inferior-link-length tmp-inferior-link-length) 200.0)
- (superior-link-weight 10.0)
- (inferior-link-weight 10.0)
- (eef-link-weight 2.0)
- (eef-body-lx 75.0)
- (eef-body-ly 50.0)
- (eef-body-lz 5.0)
- (eef-body-pos-offset (float-vector 0 0 0))
- (end-coords-offset (float-vector 0 0 0))
- )
- (send-super :init :name limb-name)
- (setq link-radius tmp-link-radius)
- (setq superior-link-length tmp-superior-link-length)
- (setq inferior-link-length tmp-inferior-link-length)
- (let* ((limb-root-link
- (instance bodyset-link :init (make-cascoords) :bodies (list (make-cube 1 1 1))
- :name (read-from-string (format nil "~A-root-link" limb-name))))
- (limb-superior-parent-pre-1-link
- (make-default-robot-link 0 link-radius :z
- (read-from-string (format nil "~A-superior-parent-pre-1-link" limb-name))))
- (limb-superior-parent-pre-2-link
- (make-default-robot-link 0 link-radius :y
- (read-from-string (format nil "~A-superior-parent-pre-2-link" limb-name))))
- (limb-superior-parent-link
- (make-default-robot-link superior-link-length link-radius :z
- (read-from-string (format nil "~A-superior-parent-link" limb-name))))
- (limb-superior-child-link
- (make-default-robot-link superior-link-length link-radius :z
- (read-from-string (format nil "~A-superior-child-link" limb-name))
- :visualize-cylinder? nil))
- (limb-inferior-parent-link
- (make-default-robot-link inferior-link-length link-radius :y
- (read-from-string (format nil "~A-inferior-parent-link" limb-name))))
- (limb-inferior-child-link
- (make-default-robot-link inferior-link-length link-radius :z
- (read-from-string (format nil "~A-inferior-child-link" limb-name))
- :visualize-cylinder? nil))
- (limb-inferior-child-post-1-link
- (make-default-robot-link 0 link-radius :z
- (read-from-string (format nil "~A-inferior-child-post-1-link" limb-name))
- :visualize-cylinder? nil))
- (limb-inferior-child-post-2-link
- (make-default-robot-link 0 link-radius :y
- (read-from-string (format nil "~A-inferior-child-post-2-link" limb-name))
- :visualize-cylinder? nil))
- (limb-eef-link)
- (limb-eef-body
- (make-cube eef-body-lx eef-body-ly eef-body-lz))
- )
- (send limb-eef-body :set-color :red)
- (send limb-eef-body :translate eef-body-pos-offset :local)
- (send limb-eef-body :worldcoords) ;; for update centroid
- (setq limb-eef-link
- (instance bodyset-link :init (make-cascoords) :bodies (list limb-eef-body)
- :name (read-from-string (format nil "~A-eef-link" limb-name)) :centroid (send limb-eef-body :centroid)))
- (setq links
- (list limb-root-link
- limb-superior-parent-pre-1-link
- limb-superior-parent-pre-2-link
- limb-superior-parent-link
- limb-superior-child-link
- limb-inferior-parent-link
- limb-inferior-child-link
- limb-inferior-child-post-1-link
- limb-inferior-child-post-2-link
- limb-eef-link
- ))
- ;; coords
- (setq root-coords (make-cascoords :rpy (list 0 0 pi) :parent limb-root-link))
- (setq middle-coords (make-cascoords :rpy (list 0 0 pi/2) :parent limb-inferior-parent-link))
- (setq end-coords (make-cascoords :name (read-from-string (format nil "~A-end-coords" limb-name))
- :pos end-coords-offset
- ;; :pos (v+ end-coords-offset (float-vector 0 0 (* -0.5 eef-body-lz))) ;; [ToDo] support eef offset
- :parent limb-eef-link))
- ;; assoc
- (send limb-inferior-child-post-2-link :assoc limb-eef-link)
- (send limb-inferior-child-post-1-link :assoc limb-inferior-child-post-2-link)
- (send limb-inferior-child-post-1-link :translate (float-vector 0 0 (- inferior-link-length)) :local)
- (send limb-inferior-child-link :assoc limb-inferior-child-post-1-link)
- (send limb-inferior-parent-link :assoc limb-inferior-child-link)
- (send limb-inferior-parent-link :translate (float-vector 0 0 (- superior-link-length)) :local)
- (send limb-superior-child-link :assoc limb-inferior-parent-link)
- (send limb-superior-parent-link :assoc limb-superior-child-link)
- (send limb-superior-parent-pre-2-link :assoc limb-superior-parent-link)
- (send limb-superior-parent-pre-1-link :assoc limb-superior-parent-pre-2-link)
- (send limb-root-link :assoc limb-superior-parent-pre-1-link)
- (send limb-root-link :rotate pi :x)
- (send self :assoc limb-root-link)
- ;; joint
- (setq joint-superior-0
- (instance rotational-joint :init :parent-link limb-root-link :child-link limb-superior-parent-pre-1-link
- :name (send limb-superior-parent-pre-1-link :name) :axis :-z :min -180 :max 180))
- (setq joint-superior-1
- (instance rotational-joint :init :parent-link limb-superior-parent-pre-1-link :child-link limb-superior-parent-pre-2-link
- :name (send limb-superior-parent-pre-2-link :name) :axis :-y :min -180 :max 180))
- (setq joint-superior-2
- (instance rotational-joint :init :parent-link limb-superior-parent-pre-2-link :child-link limb-superior-parent-link
- :name (send limb-superior-parent-link :name) :axis :-z :min -180 :max 180))
- (setq joint-linear-superior
- (instance linear-joint :init :parent-link limb-superior-parent-link :child-link limb-superior-child-link
- :name (send limb-superior-child-link :name) :axis :-z :min *-inf* :max *inf*))
- (setq joint-middle
- (instance rotational-joint :init :parent-link limb-superior-child-link :child-link limb-inferior-parent-link
- :name (send limb-inferior-parent-link :name) :axis :-y :min -180 :max 180))
- (setq joint-linear-inferior
- (instance linear-joint :init :parent-link limb-inferior-parent-link :child-link limb-inferior-child-link
- :name (send limb-inferior-child-link :name) :axis :-z :min *-inf* :max *inf*))
- (setq joint-inferior-0
- (instance rotational-joint :init :parent-link limb-inferior-child-link :child-link limb-inferior-child-post-1-link
- :name (send limb-inferior-child-post-1-link :name) :axis :-z :min -180 :max 180))
- (setq joint-inferior-1
- (instance rotational-joint :init :parent-link limb-inferior-child-post-1-link :child-link limb-inferior-child-post-2-link
- :name (send limb-inferior-child-post-2-link :name) :axis :-y :min -180 :max 180))
- (setq joint-inferior-2
- (instance rotational-joint :init :parent-link limb-inferior-child-post-2-link :child-link limb-eef-link
- :name (send limb-eef-link :name) :axis :-z :min -180 :max 180))
- (setq joint-list
- (list joint-superior-0 joint-superior-1 joint-superior-2
- joint-linear-superior joint-middle joint-linear-inferior
- joint-inferior-0 joint-inferior-1 joint-inferior-2))
- ;; weight
- (send limb-superior-parent-link :weight (* 0.5 superior-link-weight))
- (send limb-superior-child-link :weight (* 0.5 superior-link-weight))
- (send limb-inferior-parent-link :weight (* 0.5 inferior-link-weight))
- (send limb-inferior-child-link :weight (* 0.5 inferior-link-weight))
- (send limb-eef-link :weight eef-link-weight)
- (send self :_set-l-min-max-x-y limb-eef-link)
- ;; end
- (send self :init-ending)
- self
- ))
- (:_set-l-min-max-x-y
- (limb-eef-link)
- (let* ((limb-eef-link-vertices
- (mapcar #'(lambda (v) (send end-coords :inverse-transform-vector v))
- (flatten (send-all (send limb-eef-link :bodies) :vertices))))
- )
- (setq l-min-max-x-y
- (list :l-min-x (elt (find-extream limb-eef-link-vertices #'(lambda (v) (elt v 0)) #'<) 0)
- :l-max-x (elt (find-extream limb-eef-link-vertices #'(lambda (v) (elt v 0)) #'>) 0)
- :l-min-y (elt (find-extream limb-eef-link-vertices #'(lambda (v) (elt v 1)) #'<) 1)
- :l-max-y (elt (find-extream limb-eef-link-vertices #'(lambda (v) (elt v 1)) #'>) 1)
- ))
- ))
- (:l-min-max-x-y
- ()
- l-min-max-x-y
- )
- (:root-coords
- ()
- root-coords
- )
- (:middle-coords
- ()
- middle-coords
- )
- (:end-coords
- ()
- end-coords
- )
- (:arm-plane-normal
- ()
- (let* ((root-pos (send (send self :worldcoords) :worldpos))
- (middle-pos (send middle-coords :worldpos))
- (end-pos (send end-coords :worldpos))
- (outer-product-vec (v* (v- middle-pos root-pos) (v- end-pos middle-pos)))
- )
- (when (< (norm outer-product-vec) 1e-10)
- (warning-message 1 "[~a] superior-link and inferior-link are parallel.~%"
- (send (class self) :name))
- (return-from :arm-plane-normal nil)
- )
- (normalize-vector outer-product-vec)
- ))
- (:superior-link-stretched-length
- ()
- (distance (send root-coords :worldpos) (send middle-coords :worldpos))
- )
- (:inferior-link-stretched-length
- ()
- (distance (send end-coords :worldpos) (send middle-coords :worldpos))
- )
- )
- (defmethod two-stretchable-links-limb
- (:inverse-kinematics-analytical
- (tc)
- (send self :init-pose)
- (let* ((target-length (distance (send tc :worldpos) (send root-coords :worldpos)))
- (superior-length superior-link-length) (inferior-length inferior-link-length)
- (linear-angle-list) (middle-angle) (superior-angle-list) (inferior-angle-list)
- )
- (setq linear-angle-list
- (send self :_inverse-kinematics-analytical-length tc
- :target-length target-length))
- (unless (= (elt linear-angle-list 0) 0)
- (send joint-linear-superior :joint-angle (elt linear-angle-list 0))
- (setq superior-length (send self :superior-link-stretched-length))
- )
- (unless (= (elt linear-angle-list 1) 0)
- (send joint-linear-inferior :joint-angle (elt linear-angle-list 1))
- (setq inferior-length (send self :inferior-link-stretched-length))
- )
- (setq middle-angle
- (send self :_inverse-kinematics-analytical-middle tc
- :target-length target-length :superior-length superior-length :inferior-length inferior-length))
- (send joint-middle :joint-angle middle-angle)
- (setq superior-angle-list
- (send self :_inverse-kinematics-analytical-superior tc
- :target-length target-length :superior-length superior-length :inferior-length inferior-length))
- (send joint-superior-0 :joint-angle (elt superior-angle-list 0))
- (send joint-superior-1 :joint-angle (elt superior-angle-list 1))
- (setq inferior-angle-list
- (send self :_inverse-kinematics-analytical-inferior tc))
- (send joint-inferior-0 :joint-angle (elt inferior-angle-list 0))
- (send joint-inferior-1 :joint-angle (elt inferior-angle-list 1))
- (send joint-inferior-2 :joint-angle (elt inferior-angle-list 2))
- (send self :angle-vector)
- ))
- (:_inverse-kinematics-analytical-length
- (tc
- &key
- (target-length (distance (send tc :worldpos) (send root-coords :worldpos)))
- )
- (let* ((linear-superior-angle 0)
- (linear-inferior-angle 0)
- )
- (cond ((and (< target-length (+ superior-link-length inferior-link-length))
- (> target-length (abs (- superior-link-length inferior-link-length))))
- )
- ((<= target-length (abs (- superior-link-length inferior-link-length)))
- (cond ((> superior-link-length inferior-link-length)
- (setq linear-superior-angle
- (- target-length (abs (- superior-link-length inferior-link-length))))
- )
- (t
- (setq linear-inferior-angle
- (- target-length (abs (- superior-link-length inferior-link-length))))
- ))
- )
- ((>= target-length (+ superior-link-length inferior-link-length))
- (setq linear-inferior-angle (* 0.5 (- target-length (+ superior-link-length inferior-link-length))))
- (setq linear-superior-angle (* 0.5 (- target-length (+ superior-link-length inferior-link-length))))
- ))
- (list linear-superior-angle linear-inferior-angle)
- ))
- (:_inverse-kinematics-analytical-middle
- (tc
- &key
- (target-length (distance (send tc :worldpos) (send root-coords :worldpos)))
- (superior-length (send self :superior-link-stretched-length))
- (inferior-length (send self :inferior-link-stretched-length))
- )
- (let* ((middle-angle
- (acos (min 1 (max -1
- (/ (- (expt target-length 2) (expt superior-length 2) (expt inferior-length 2))
- (* 2.0 superior-length inferior-length))))))
- )
- (rad2deg middle-angle)
- ))
- (:_inverse-kinematics-analytical-superior
- (tc
- &key
- (target-length (distance (send tc :worldpos) (send root-coords :worldpos)))
- (superior-length (send self :superior-link-stretched-length))
- (inferior-length (send self :inferior-link-stretched-length))
- )
- (let* ((tc-local (send root-coords :transformation tc))
- (x (elt (send tc-local :pos) 0))
- (y (elt (send tc-local :pos) 1))
- (z (elt (send tc-local :pos) 2))
- (xyz-norm (norm (float-vector x y z)))
- (xy-norm (norm (float-vector x y)))
- (theta
- (if (eps= xyz-norm 0 1e-10) 0.0 (acos (/ z (norm (float-vector x y z))))))
- (phi
- (if (eps= xy-norm 0 1e-10) 0.0 (* (if (> y 0) 1 -1) (acos (/ x xy-norm)))))
- (theta-offset
- (acos (min 1 (max -1
- (/ (+ (expt target-length 2) (expt superior-length 2) (* -1 (expt inferior-length 2)))
- (* 2.0 target-length superior-length))))))
- )
- (list (rad2deg phi) (rad2deg (- theta theta-offset)))
- ))
- (:_inverse-kinematics-analytical-inferior
- (tc)
- (let* ((end2middle-vec
- (v- (send middle-coords :worldpos) (send tc :worldpos)))
- (end2middle-vec-local
- (send tc :inverse-rotate-vector end2middle-vec))
- (x (elt end2middle-vec-local 0))
- (y (elt end2middle-vec-local 1))
- (z (elt end2middle-vec-local 2))
- (theta
- (acos (/ z (norm (float-vector x y z)))))
- (phi
- (* (signum y) (acos (/ x (norm (float-vector x y))))))
- (joint-middle-axis
- (send (send (send joint-middle :child-link) :worldcoords)
- :rotate-vector (float-vector 0 1 0)))
- (transed-end-axis
- (send (send (send (send tc :copy-worldcoords) :rotate phi :z) :rotate theta :y)
- :rotate-vector (float-vector 0 1 0)))
- (yaw
- (vector-angle joint-middle-axis transed-end-axis (scale -1 (normalize-vector end2middle-vec))))
- )
- (list (rad2deg yaw) (rad2deg theta) (rad2deg phi))
- ))
- )
- ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
- ;; test
- ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
- (defun test-two-stretchable-links-limb-ik
- (&key
- (wait? nil)
- )
- (dotimes (i 100)
- (let* ((tc (make-coords :pos (scale (random 1000) (random-vector)) :rpy (scale (random pi) (random-vector))))
- )
- (setq *limb* (instance two-stretchable-links-limb :init
- :superior-link-length (+ (random 100.0) 100.0) :inferior-link-length (+ (random 0.0) 100.0)))
- (test-two-stretchable-links-limb-ik-one tc)
- (when wait? (read-line))
- ))
- (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
- (test-two-stretchable-links-limb-ik-one (make-coords))
- (when wait? (read-line))
- (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 200 :inferior-link-length 100))
- (test-two-stretchable-links-limb-ik-one (make-coords))
- (when wait? (read-line))
- (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 200))
- (test-two-stretchable-links-limb-ik-one (make-coords))
- (when wait? (read-line))
- (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
- (test-two-stretchable-links-limb-ik-one (make-coords :pos (float-vector 0 0 100)))
- (when wait? (read-line))
- (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
- (test-two-stretchable-links-limb-ik-one (make-coords :pos (float-vector 0 0 200)))
- (when wait? (read-line))
- (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
- (test-two-stretchable-links-limb-ik-one (make-coords :pos (float-vector 0 0 300)))
- (when wait? (read-line))
- (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
- (test-two-stretchable-links-limb-ik-one (make-coords :pos (float-vector 100 0 0)))
- (when wait? (read-line))
- (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
- (test-two-stretchable-links-limb-ik-one (make-coords :pos (float-vector -100 0 0)))
- (when wait? (read-line))
- (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
- (test-two-stretchable-links-limb-ik-one (make-coords :pos (float-vector 0 100 0)))
- (when wait? (read-line))
- (setq *limb* (instance two-stretchable-links-limb :init :superior-link-length 100 :inferior-link-length 100))
- (test-two-stretchable-links-limb-ik-one (make-coords :pos (float-vector 0 -100 0)))
- (when wait? (read-line))
- )
- (defun test-two-stretchable-links-limb-ik-one
- (tc)
- (send *limb* :inverse-kinematics-analytical tc)
- (objects (list *limb*))
- (send tc :draw-on :flush t :size 75 :width 5 :color #f(1 0.5 0.5))
- (send (send *limb* :end-coords) :draw-on :flush t :size 50 :width 10 :color #f(0.5 1 0.5))
- (assert (eps-coords= tc (send *limb* :end-coords)))
- )
- (warn "(test-two-stretchable-links-limb-ik)~%")
Add Comment
Please, Sign In to add comment