2.3 �r���\�������N���X

manipulator.l�����C �r�S�����\�������N���X������manipulator�� �������������`�����`�����������D

;;;; manipulator.l
;;;; class JOINT and MANIPULATOR
;;;;        defines linked articulation
;;;;        Copyright (c) 1988,
;;;;      Toshihiro MATSUI,
;;;       Electrotechnical Laboratory
;;;;

;; A manipulator is assoc'ed to a body
;; or an instance of cascaded-coords
;; which forms the base of a linked
;; manipulator structure. A manipulator
;; manages the transformation from
;; the base frame to the toolcoords.
;; Arm-solution method must be defined
;; to the transformation between the
;; base and the hand-coords.

(defclass manipulator :super cascaded-coords
  :slots
  (base;;base object
   joints;;joint objects (instances of mmdjoint)
   hand;;hand object
   right-finger;; body
   left-finger;; body
   (toolcoords    ;;offset from hand to tool
    :type cascaded-coords)
   (toolinverse :type coordinates)
   (default-toolcoords :type cascaded-coords)
   (handcoords :type coordinates)
   (armsolcoords :type coordinates)
   (baseinverse :type coordinates)
   openvec ;;direction for opening fingers
   (max-span :type :float)
   (angles :type float-vector) ;;joint angles 
   (newangles :type float-vector)
   (tempcoords ;;temporary coordinates
    :type coordinates) 
   act;;boolean for simulation control
   approach-config
   grasp-config
   affixed-object
   lefty;;three configuration mode flags
   flip;;
   below;;
   components
   update-components
   ))

(defun manipulator-p (obj)
  (derivedp obj manipulator))

;;;
;;; coordinates and angles management
;;;

(defmethod manipulator
  (:newcoords
   (c &optional p)
   (send-super :newcoords c p)
   (send self :armsolcoords)
   (cond
     ((send self :armsol armsolcoords)
      ;;possible configuration?
      ;; armsol finds solution in newangles
      (cond ((send self :config newangles)
             (send tempcoords :replace-coords self)
             t)
        (t nil)))
     (t
    ;;no arm solution for this configuration
      (warning-message 3 "armsol failed")
      ;;;restore old config.
      (send self :replace-coords tempcoords)
      nil)))
  (:armsol
   (&rest mesg)
   (send self
         :error
         ":armsol subclass'es responsibility"))
  (:armsolcoords
   ()
   (transform-coords
    self toolinverse armsolcoords))
  (:tool (&rest msg);;modify and get toolcoords
     (when msg
       (apply #'send toolcoords msg)
       (send (send toolcoords :copy-coords)
             :inverse-transformation toolinverse))
     toolcoords)
  (:toolinverse () toolinverse)
  (:set-tool
   (newtool &optional offset copy)
   (if copy
       (setq newtool (cascoords :coords newtool)))
   (if toolcoords
       (send handcoords :dissoc toolcoords))
   (setq toolcoords newtool)
   (send handcoords :assoc toolcoords offset)
   (setq offset (send toolcoords :copy-coords) )
   (setq toolinverse
         (send offset :inverse-transformation))
   (send self :replace-coords
         (send base :transformation
               (send toolcoords :worldcoords)))
   (send tempcoords :replace-coords self)
   (send self :changed)
   toolcoords)
  (:reset-tool
   ()
   ;;  (when (not (eq toolcoords default-toolcoords))
   ;;        (send handcoords :dissoc toolcoords)
   ;;        (send default-toolcoords :worldcoords)
   (send self :set-tool
         default-toolcoords default-toolcoords t)
   toolcoords)
  (:worldcoords
   ()
   (if update-components
       (send-all components :worldcoords))
   (send-super :worldcoords))
  (:set-config-mode () nil)
  (:set-coords
   ();;force setting coords
     ;;according to the forward kinematics
   (let
       ((trans 
         (send base :transformation
             (send toolcoords :worldcoords))))
     (send self :replace-coords trans)
     (send self :set-config-mode)) )
  (:config  ;;update joint angles
   (&optional (a newangles))
   (unless (eq a newangles)
           (replace newangles a))
   (dotimes (i 6)
     (unless
         (send (elt joints i)
             :angle-in-bounds-p (elt newangles i) )
       (warning-message
        1 "joint ~d angle exceeded~%" i)
    ;;;  (return-from :config nil)
    ))
   (dotimes (i 6)
      (send (elt joints i)
            :orient-axis (elt newangles i)))
   ;;config succeeded, updata angles
   (replace angles newangles)
   angles)
  (:park ()
         (send self :config
               (float-vector 0 0 0 0 0 0))
         (send self :set-coords)))

;; configuration flags
(defmethod manipulator
  (:lefty ()  (setq lefty t))
  (:righty () (setq lefty nil))
  (:below ()  (setq below t))
  (:above ()  (setq below nil))
  (:flip ()   (setq flip  t))
  (:noflip () (setq flip  nil))
  ;;
  (:leftyp () lefty)
  (:belowp () below)
  (:flipp  () flip)
  ;;
  (:config-mode () (list lefty below flip))
  )


;;
;; hand and gripper
;;

(defmethod manipulator
  (:hand (&optional (h nil))
         (if h (setq hand h) hand))
  (:handcoords () handcoords)
  (:newhand
   (nh hc)
   (setq hand nh)
   (setq handcoords (make-cascoords :coords  hc)) 
   (send hand :assoc handcoords handcoords)
   )
  (:span
   ()
  ;;returns the current distance between fingers
   (norm (v- (send right-finger :pos)
             (send left-finger :pos))))
  (:open-fingers
   (s &optional abs
      &aux (current (send self :span)))
   ;; move fingers relatively or absolutely
   (if abs  (setq s (- s current)))
   (setq abs (+ current s))
   (when (or (< abs 0.0) (> abs max-span))
     (return-from
         :open-fingers "cannot open fingers"))
   (setq s (/ s 2.0))
   (send right-finger
         :translate (scale s openvec))
   (send left-finger
         :translate (scale (- s) openvec))    
   abs)
  (:close-fingers
   () (send self
            :open-fingers (- (send self :span)))) )

;;
;; accessing
;;
(defmethod manipulator
  (:angles
   (&optional flag)
   (if flag
       (send-all joints :angle)
     angles))
  (:axis () (send-all joints :axis))
  (:act (flag) (prog1 act (setq act flag)))
  (:get-approach ()  approach-config)
  (:set-approach (a) (setq approach-config a))
  (:get-grasp () grasp-config)
  (:set-grasp (g) (setq grasp-config g))
  (:get-affix () affixed-object)
  (:joints () joints)
  (:components
   ()
   (if affixed-object
       (cons affixed-object components)
     components))
  (:drawners
   ()
   (if hand 
       (funcall #'append (send self :components)
                (if (find-method hand :drawners)
                    (send hand :drawners)
                  (list hand)))
     (send self :components)) )
  (:base
   (&rest mesg)
   (if mesg
       (prog1
           (send-lexpr base mesg)
         (setq baseinverse
               (send (send base :worldcoords)
                     :inverse-transformation)))
     base) )
  )

;;
;; affixment
;; only logical relations between
;; the grasped part and the hand is handled.

(defmethod manipulator
  (:affix
   (&optional (grasp))
   (if (null grasp)
      (setq grasp (send grasp-config :parent)))
   (when (and grasp
              (not (eq grasp affixed-object)))
     (send hand :assoc grasp)
     (setq affixed-object grasp))
   affixed-object)
  (:unfix
   (&optional (margin 10.0))
   (if affixed-object
       (send hand :dissoc affixed-object))
   (setq affixed-object nil
         approach-config grasp-config
         grasp-config  nil)
   )
  )

;;
;; creation
;;

(defmethod manipulator
(:create
 (&rest args
  &key ((:name nm)) ((:hand h)) ((:joints j))
       ((:left-finger lf)) ((:right-finger rf))
       ((:toolcoords tc) (make-coords))
       ((:handcoords hc) (make-coords))
       ((:base bs) (make-cascoords))
       (open-direction (floatvector 0 1 0))
       ((:max-span mspan) 100.0)
       ((:lefty lft) t)
       ((:act a) nil)
  &allow-other-keys)
     (send-lexpr self :init args)
     (setq tempcoords (make-coords)
           right-finger rf
           left-finger  lf
           joints j
           angles (instantiate float-vector 6)
           newangles (instantiate float-vector 6)
           act a
           openvec open-direction
           max-span mspan
           lefty lft
           base bs
           armsolcoords (make-coords)
           baseinverse
               (send base
                     :inverse-transformation))
     (send self :newhand h hc)
     (setq default-toolcoords tc)
     (send self :set-tool tc tc t)
     (send base :assoc self self)
     (setq components
           (remove-duplicates
            (append (list base)
                    joints
                    (list hand
                      left-finger right-finger))))
     (setq components (delete nil components))
     (send-all components :manager self)
     (send self :config)
     self))

;;
;; E T A 3  manipulator
;;
(defclass eta3arm :super manipulator)

(defmethod eta3arm
  (:armsol
   (c);;nil if impossible configuration
   (eta3armsol  (coordinates-rot c)
                (coordinates-pos c)
                angles (not lefty) newangles) )
  (:set-config-mode
   () 
   ;;    (setq lefty (>= (elt angles 2) 0.0))) 
   (setq lefty (<= (elt angles 2) 0.0)))
  ;; changed by H. Nakagaki 05-Apr-1996
  (:park
   ()
   (send self
         :config (float-vector -0.05
                               0
                               -0.05
                               (- pi/2)
                               0
                               pi/2))
   (send self :set-coords)))