Next: 3.2 マニピュレータの定義
Up: 3 ロボットの形状モデル
Previous: 3 ロボットの形状モデル
ロボットの各リンクを形状モデラで生成する手順を説明するために図
9を示す．図では，リンク４を例として，角柱
(Prism)と直方体(Cube) の差集合演算(Body-)を行って作る．各立体を作る際
には基本立体の演算の関係から，その立体の位置と姿勢座標は自由に設定して
作り，その後基準座標系を各リンク
の座標系の位置に設定する．す
べてのリンクをアーム基準座標系
と同じ方向に並べたものが図
9右である．
;;;
;;; Element Parts of Standard Manipulator
;;; for Intelligent Robotics
;;; JSK, The University of Tokyo
;;;
;;; 2004.12.13 created by M. Inaba
;;;
(defun make-sarm-base nil
(let ((b1 (make-cube 180 136 167))
(c (make-cylinder 35 65.5))
(b0 (make-prism
(list
(float-vector -140 700 -20)
(float-vector 700 700 -20)
(float-vector 700 -700 -20)
(float-vector -140 -700 -20))
20))
)
(send b1 :locate
(float-vector -50
0
(- (/ 165 2.0) 1.0)))
(send c :locate (float-vector 0 0 164.5))
(setq b1 (body+ b1 c))
(setq b1 (body+ b1 b0))
(send b1 :worldcoords)
b1))
(setq sarm-b0 (make-sarm-base))
(defun make-sarm-l1 nil
(let ((b (make-cube 80 80 110)))
(send b :locate
(float-vector 0 0 (+ 55 230)))
))
(defun make-sarm-l1 nil
(let* ((h 136)
(h/2 (/ h -2))
(b (make-prism
(list
(float-vector 80 40 h/2)
(float-vector 80 -40 h/2)
(float-vector 0 -40 h/2)
(float-vector -30 -30 h/2)
(float-vector -30 30 h/2)
(float-vector 0 40 h/2))
h))
(d 100)
(d/2 (/ d -2))
(b1
(make-prism
(list
(float-vector 70 50 d/2)
(float-vector 70 -50 d/2)
(float-vector -50 -50 d/2)
(float-vector -50 50 d/2)
)
d))
)
(setq b (body- b b1))
(send b :translate-vertices #f(-80 0 0))
(send b :rotate-vertices pi/2 :x)
(send b :rotate-vertices pi/2 :y)
(send b :translate-vertices #f(0 0 230))
(send b :worldcoords)
b)
)
(setq sarm-b1 (make-sarm-l1))
(defun make-sarm-l2 nil
(let*
((b (make-prism
(list
(float-vector 0 40 -40)
(float-vector 30 30 -40)
(float-vector 40 15 -40)
(float-vector 40 -15 -40)
(float-vector 30 -30 -40)
(float-vector 0 -40 -40)
(float-vector -150 -40 -40)
(float-vector -300 -30 -40)
(float-vector
(- (+ 300 (* 30 (sin #d30))))
(- (* 30 (cos #d30)))
-40)
(float-vector
(- (+ 300 (* 30 (sin #d60))))
(- (* 30 (cos #d60)))
-40)
(float-vector
(- (+ 300 (* 30 (sin #d90))))
(- (* 30 (cos #d90)))
-40)
(float-vector
(- (+ 300 (* 30 (sin #d60))))
(* 30 (cos #d60))
-40)
(float-vector
(- (+ 300 (* 30 (sin #d30))))
(* 30 (cos #d30))
-40)
(float-vector -300 30 -40)
(float-vector -150 40 -40)
)
80))
(b1 (make-prism
(list
(float-vector -180 50 -35)
(float-vector -180 -50 -35)
(float-vector -350 -50 -35)
(float-vector -350 50 -35))
70))
)
(setq b (body- b b1))
(send b :rotate-vertices pi/2 :z)
(send b :worldcoords)
b)
)
(setq sarm-b2 (make-sarm-l2))
(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))
(defun make-sarm-l4 nil
(let
((b (make-prism
(list
(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 #d120))
(* 30 (cos #d120))
-30)
(float-vector
(* 30 (sin #d150))
(* 30 (cos #d150))
-30)
(float-vector 0 -30 -30)
(float-vector -257 -30 -30)
(float-vector -257 30 -30)
)
60))
(b1 (make-prism
(list
(float-vector -30 40 -26)
(float-vector 40 40 -26)
(float-vector 40 -40 -26)
(float-vector -30 -40 -26)
)
52))
)
(setq b (body- b b1))
(send b :translate-vertices #f(400 0 0))
(send b :worldcoords)
(send b :rotate-vertices pi/2 :x)
(send b :rotate-vertices -pi/2 :y)
(send b :worldcoords)
b))
(setq sarm-b4 (make-sarm-l4))
(defun make-sarm-l5 nil
(let
((b (make-prism
(list
(float-vector
(* 24 (sin #d0))
(* 24 (cos #d0)) -25)
(float-vector
(* 24 (sin #d30))
(* 24 (cos #d30)) -25)
(float-vector
(* 24 (sin #d60))
(* 24 (cos #d60)) -25)
(float-vector
(* 24 (sin #d90))
(* 24 (cos #d90)) -25)
(float-vector
(* 24 (sin #d120))
(* 24 (cos #d120)) -25)
(float-vector
(* 24 (sin #d150))
(* 24 (cos #d150)) -25)
(float-vector
(* 24 (sin #d180))
(* 24 (cos #d180)) -25)
(float-vector
(* 24 (sin #d210))
(* 24 (cos #d210)) -25)
(float-vector
(* 24 (sin #d240))
(* 24 (cos #d240)) -25)
(float-vector
(* 24 (sin #d270))
(* 24 (cos #d270)) -25)
(float-vector
(* 24 (sin #d300))
(* 24 (cos #d300)) -25)
(float-vector
(* 24 (sin #d330))
(* 24 (cos #d330)) -25)
)
50))
(b1 (make-prism
(list
(float-vector 0 19 -17)
(float-vector 34 19 -17)
(float-vector 34 -19 -17)
(float-vector 0 -19 -17)
)
34))
)
(setq b (body+ b b1))
(send b :rotate-vertices pi/2 :z)
(send b :worldcoords)
b)
)
(setq sarm-b5 (make-sarm-l5))
(defun make-sarm-l6 nil
(let ((b (make-prism
(list
(float-vector 35 20 -15)
(float-vector 80 20 -15)
(float-vector 80 -20 -15)
(float-vector 35 -20 -15)
)
30))
(b1 (make-prism
(list
(float-vector 75 55 -13)
(float-vector 100 55 -13)
(float-vector 100 -55 -13)
(float-vector 75 -55 -13)
)
26))
(b2 (make-prism
(list
(float-vector 78 53 -12)
(float-vector 110 53 -12)
(float-vector 110 -53 -12)
(float-vector 78 -53 -12)
)
24))
)
(setq b (body+ b b1))
(setq b (body- b b2))
(send b :rotate-vertices -pi/2 :y)
(send b :worldcoords)
b))
(setq sarm-b6 (make-sarm-l6))
(defun make-sarm-finger-r nil
;; 40mm 10mm 19mm
(let ((b (make-prism
(list
(float-vector 80 -0.5 -9.5)
(float-vector 140 -0.5 -9.5)
(float-vector 140 -5 -9.5)
(float-vector 100 -10 -9.5)
(float-vector 80 -10 -9.5)
)
19)))
(send b :translate-vertices #f(-140 0 0))
(send b :rotate-vertices -pi/2 :y)
(send b :rotate-vertices pi :z)
(send b :worldcoords)
b)))
(setq sarm-fr (make-sarm-finger-r))
(defun make-sarm-finger-l nil
;; 40mm 10mm 19mm
(let ((b (make-prism
(list
(float-vector 80 10 -9.5)
(float-vector 100 10 -9.5)
(float-vector 140 5 -9.5)
(float-vector 140 0.5 -9.5)
(float-vector 100 0.5 -9.5)
)
19))
)
(send b :translate-vertices #f(-140 0 0))
(send b :rotate-vertices -pi/2 :y)
(send b :rotate-vertices pi :z)
(send b :worldcoords)
b))
(setq sarm-fl (make-sarm-finger-l))
(setq sarmbodies
(list sarm-b0 sarm-b1 sarm-b2
sarm-b3 sarm-b4 sarm-b5
sarm-b6 sarm-fr sarm-fl))
generated through LaTeX2HTML. M.Inaba 平成18年5月7日