2.1 関節のクラス

Next: 2.2 関節の定義マクロ　defjoint Up: 2 マニピュレータ表現 Previous: 2 マニピュレータ表現

2.1 関節のクラス

関節クラスrotational-jointをリンクの形状を表現するbodyへのメソッドを継承する形にすることを考えて，bodyのサブクラスとするとして，次のように定義を行う．


(defclass rotational-joint
  :super body
  :slots (axis       ;回転軸 (:x,:y,:z)
         offset      ;親の軸からのオフセット
         high-limit  ;最大可動回転角(rad)
         low-limit   ;最小可動回転角(rad)
         ))

(defmethod rotational-joint
  (:create
   (&key
    name 
    ((:parent par))
    (shape)
    ((:axis ax) :z)
    ((:high-limit hl)  pi)
    ((:low-limit lo)  -pi)
    ((:offset ofs) (make-coords))
    (wrt :parent)
    color
    &allow-other-keys)
   (when shape
     (replace-object self shape)
     (send-all faces :body self))
   (setq axis ax
         high-limit hl low-limit lo
         offset ofs)
   (if name (send self :name name))
   (if par (send par :assoc self))
   (send self :locate (coordinates-pos ofs) wrt)
   (send self :color color)
   self
   ))

関節のインスタンスを生成するメソッドとして， :createを定義する． :createに:shapeを与えるとボディのデータを与えられたデータで置き換える．


(defparameter *joint-class* rotational-joint)

(defmacro defjoint (name &rest initargs
                      &key (class *joint-class*)
                      &allow-other-keys)
  `(setq ,name 
        (send (instantiate ,class) :create
              :name ',name ,@initargs)))


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

(defun fold-angle (r)
  (while (< r -pi) (incf r 2pi))
  (while (> r pi) (decf r 2pi))
  r))

(defclass rotational-joint
  :super body
  :slots
  (axis ;;rotational-axis (:x,:y,:z)
   offset ;;offset from the parent joint
   high-limit ;;rotatable angle range
   low-limit ;;rotatable angle range
   ))

(defmethod rotational-joint
  (:angle-in-bounds-p
   (th)
   (< low-limit th high-limit))
  (:orient-axis
   (th)        ;;;th is absolute angle (radian)
   (rotate-matrix (coordinates-rot offset)
                  th axis nil rot)
   (send self :newcoords rot pos)
   self)
  (:rotate-axis
   (dth);;rotate axis incrementally
   (rotate-matrix rot dth axis nil rot)
   (send self :newcoords rot pos)
   self)
  (:axis (&optional (a nil))
         (if  a (setq axis a))
         axis)
  (:angle
   ()
   (elt
    (car (send self :rpy-angle))
    (cdr (assoc axis
                '((:x . 2) (:y . 1) (:z . 0))
                ))))
  (:set-angle-limit
   (l h)
   (setq low-limit l   high-limit h)
   self)
  (:limit () (list low-limit high-limit))
  (:offset (&rest mesg)  (resend offset mesg))
  (:create
   (&key name 
         ((:parent par))
         (shape)
         ((:axis ax) :z)
         ((:high-limit hl)  pi)
         ((:low-limit lo)  -pi)
         ((:offset ofs) (make-coords))
         (wrt :parent)
         color
         &allow-other-keys)
   (when shape
     (replace-object self shape)
     (send-all faces :body self))
   (setq axis ax
         high-limit hl
         low-limit lo
         offset ofs)
   (if name (send self :name name))
   (if par (send par :assoc self))
   (send self :locate (coordinates-pos ofs) wrt)
   (send self :color color)
   self
   ))

(defparameter *joint-class* rotational-joint)

(defmacro defjoint
  (name &rest initargs
        &key (class *joint-class*)
        &allow-other-keys)
  `(setq ,name 
         (send (instantiate ,class) :create
               :name ',name ,@initargs)))

たとえば，


(defun make-sarm-l3 nil
  (let
      ((b
        (make-prism
         (list
          (float-vector 142 30 -30)
          (float-vector 142 -30 -30)
          (float-vector 0 -30 -30)
          (float-vector (- (* 30 (sin #d30)))
                        (- (* 30 (cos #d30)))
                        -30)
          (float-vector (- (* 30 (sin #d60)))
                        (- (* 30 (cos #d60)))
                        -30)
          (float-vector (- (* 30 (sin #d90)))
                        (- (* 30 (cos #d90)))
                        -30)
          (float-vector (- (* 30 (sin #d60)))
                        (* 30 (cos #d60))
                        -30)
          (float-vector (- (* 30 (sin #d30)))
                        (* 30 (cos #d30))
                        -30)
          (float-vector 0 30 -30)
          )
         60)))
    (send b :rotate-vertices pi/2 :z)
    (send b :worldcoords)
    b))

(setq sarm-b3 (make-sarm-l3))

generated through LaTeX2HTML. M.Inaba 平成18年5月7日