4.3 �n�m�C���������� sample/hanoimanipulation.l

�n�m�C����������

(defun move-on (o to)
  (send o
        :locate
        (v+ (send (get to :top-coords) :worldpos)
            #f(0 0 0.05))
        :world)
  )

(defmethod manipulator
  (:release-object
   (&optional (g (get self :grasped-object))
              draw)
   (cond
       (g
        (send self :open-fingers 80 :abs t)
        (setf (get self :grasped-object) nil)
        (send handcoords :dissoc g)
        t)
     (t (warn "no grasped object~%"))
     )
   (if draw (eval draw))
   )
  (:move-object
   (obj on &key draw)
   (let ((g (get self :grasped-object)))
     (when (and g (not (eq g obj)))
       (send self :release-object))
     (send self :grasp-object obj)
     (send self :place-object on)
     (if draw (eval draw))
     )
   )
  (:place-object
   (support-object
    &key (target (get self :grasped-object))
    release draw)
   (cond
       ((null target) 
        (warn "no grasped object~%"))
     ((derivedp support-object body)
      (move-on target support-object)
      (send self :grasp-object target)
      (cond
          (release
           (send self :release-object target))
        (t (send handcoords :assoc target)
           (send handcoords :worldcoords))))
     (t nil))
   (send self :worldcoords)
   (if draw (eval draw))
   )
  (:grasp-object
   (o &key draw)
   (if (get self :grasped-object)
       (send self :release-object))
   (when (get o :grasp-width)
     (send self :open-fingers
          (get o :grasp-width) :abs t)
     (send self :move-hand o)
     (setf (get self :grasped-object) o)
     (send handcoords :assoc o)
     (send handcoords :worldcoords))
   (send self :worldcoords)
   (if draw (eval draw))
   )
  (:move-hand
   (obj)
   (send self :move-hand-to-pos (send obj :centroid))
   )
  )