two-stretchable-links-limb

two-stretchable-links-limb.l

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; 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)~%")