Next: 4 物体操作の記述
Up: 3 ロボットの形状モデル
Previous: 3.1 リンク形状の定義
マニピュレータの定義は，
リンクの形状を定義した後，それを用いて
関節（ジョイント）を定義し，ジョイント全体を
まとめるマニピュレータクラスを定義することで行う．
(require 'manipulator "robot/manipulator")
(load "sarmmodel.l")
(defjoint sarm0
:shape sarm-b0
:color 3
)
(defjoint sarm1
:shape sarm-b1
:color 3
:parent sarm0
:axis :z
:offset (make-coords :pos (float-vector 0 0 0))
:high-limit (deg2rad 85)
:low-limit (deg2rad -85)
)
(defjoint sarm2
:shape sarm-b2
:color 4
:parent sarm1
:axis :z
:offset (make-coords :pos #f(0 0 310)
:rot (m* (rotation-matrix pi/2 :y)
(rotation-matrix -pi/2 :x)))
:low-limit (deg2rad -120)
:high-limit (deg2rad -30)
)
(defjoint sarm3
:shape sarm-b3
:color 4
:parent sarm2
:axis :z
:offset (make-coords :pos #f(0 -300 0)
:rot (rotation-matrix -pi/2 :z))
:low-limit (deg2rad -30) :high-limit (deg2rad 60)
)
(defjoint sarm4
:shape sarm-b4
:color 4
:parent sarm3
:axis :z
:offset (make-coords :pos #f(0 0 0)
:rot (rotation-matrix -pi/2 :x))
:low-limit (deg2rad -165) :high-limit (deg2rad 165)
)
(defjoint sarm5
:shape sarm-b5
:color 4
:parent sarm4
:axis :z
:offset (make-coords :pos #f(0 0 400)
:rot (rotation-matrix pi/2 :x)
)
:low-limit (deg2rad -90) :high-limit (deg2rad 90)
)
(defjoint sarm6
:shape sarm-b6
:color 1
:parent sarm5
:axis :z
:offset (make-coords :pos #f(0 0 0)
:rot (rotation-matrix -pi/2 :x))
:low-limit (deg2rad -180) :high-limit(deg2rad 180)
)
(defjoint sarm7
:shape sarm-fr
:color 2
:parent sarm6
:offset (make-coords :pos #f(0 0 140))
)
(defjoint sarm8
:shape sarm-fl
:color 2
:parent sarm6
:offset (make-coords :pos #f(0 0 140))
)
(defclass sarmclass :super manipulator)
(defmethod sarmclass
(:park ()
(send self :config (float-vector 0 #d-90 0 0 0 0))
(send self :set-coords))
(:armsol
(&optional (c (send self :handcoords)))
(let* (
(cc (send c :copy-worldcoords))
(h-1 (make-coords :pos #f(0 0 -140)
:rot (rotation-matrix pi/2 :z)))
(s (send self :shoulder :worldpos))
(u6 (send cc :transform h-1))
(w (send u6 :worldpos))
(r6 (send u6 :worldrot))
(l0 (elt s 2))
(l1 300) (l2 400)
(wx (elt w 0))
(wy (elt w 1))
(wz (elt w 2))
(d (distance w s))
phi3 phi2 phi1 psi2 th1 th2 th3 th4 th5 th6
u3 u4 u5 u5z u3x u3y u3z
(u6x (matrix-column r6 0))
(u6y (matrix-column r6 1))
(u6z (matrix-column r6 2))
)
(setq phi3 (acos (/ (- (* d d) (* l1 l1) (* l2 l2))
(* 2 l1 l2))))
(setq psi2 (atan (* l2 (sin phi3))
(+ l1 (* (cos phi3) l2))))
(setq phi2
(atan (- wz l0) (sqrt (+ (* wx wx) (* wy wy)))))
(setq phi1 (atan wy wx))
(setq th1 phi1)
(setq th2 (- (+ psi2 phi2)))
(setq th3 (- phi3 pi/2))
;;
(send (elt joints 0) :orient-axis th1)
(send (elt joints 1) :orient-axis th2)
(send (elt joints 2) :orient-axis th3)
(setq u3 (send (elt joints 2) :worldrot))
(setq u3x (matrix-column u3 0))
(setq u3y (matrix-column u3 1))
(setq u3z (matrix-column u3 2))
;; (setq th5 (- (acos (v. u3x u6z)) pi/2))
(setq th5 (acos (v. u3y u6z)))
(if (< (v. u6x u3y) 0) (setq th5 (- th5)))
(cond
((> th5 0)
(setq th4 (atan (v. u3z u6z)
(- (v. u3x u6z))))
(setq th6 (atan (- (v. u3y u6y))
(v. u3y u6x))))
(t (setq th4 (atan (- (v. u3z u6z))
(v. u3x u6z)))
(setq th6 (atan (v. u3y u6y)
(- (v. u3y u6x))))))
(setq newangles
(float-vector th1 th2 th3 th4 th5 th6))
(send self :config newangles)
(send self :set-coords)
newangles)
)
(:handpos nil (send (send self :handcoords) :worldpos))
(:handrot nil (send (send self :handcoords) :worldrot))
(:shoulder
(&rest args)
(if args (send* (elt joints 1) args) (elt joints 1))
)
(:elbow
(&rest args)
(if args (send* (elt joints 2) args) (elt joints 2))
)
(:wrist
(&rest args)
(if args (send* (elt joints 4) args) (elt joints 4))
)
)
(defmanipulator sarm
:class sarmclass
:base sarm0
:joints (list sarm1 sarm2 sarm3 sarm4 sarm5 sarm6)
:hand sarm6
:left-finger sarm7
:right-finger sarm8
:handcoords (make-coords :pos #f(0 0 140)
:rot (rotation-matrix -pi/2 :z))
:lefty nil
:toolcoords (coords :pos #f(0 0 0))
:open-direction #f(0 -1 0)
)
(send sarm :act t)
(send sarm :park)
(send sarm :open-fingers 30)
generated through LaTeX2HTML. M.Inaba 平成18年5月7日