;;;; 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)))