知能システム論 1 (9) 2015.6.17 情報システム学研究科情報メディアシステム学専攻知能システム学講座末廣尚士
13. アームモデルの Python による表現 理想ロボット :ArmWithHand 構造は関係なし move: 手先や持った物を動かす ハンド :Hand open, close, width アームのリンクの計算 :Link set_jparam シリアルリンクアーム :LinkedArm set_joints 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 2
- 方針 多少 贅沢 ( 計算量のことは気にしない ) でよいから 見通しの良さ 一般性 汎用性 形状モデルの導入 larm_w_hand_sol.py 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 3
- アームモデルの構造 link を PartsObject ( 座標系 CoordinateObject) で表現する 次のlinkは前のlinkに連鎖させる これにより関節の動作を次のlinkの位置を変化させるように連鎖できることになる jointは動かされる座標系の原点ににあり その相対座標変換を変化させることで実現する jointは指定された1 軸の並進または回転をすることができる 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 4
- link の表現 PartsObjectを継承したclass とする baseを前のリンクの適切な場所に置く x, y, z 方向の並進 軸回りの回転のjointを持つ joint parameterによって baseに対するlink frame の相対座標が変化する link frame joint base joint parameter 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 5
- link joint の例 回転関節 並進関節 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 6
- link frame の配置イメージ 関節 (joint) リンク (link) 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 7
- link の表現 PartsObjectを継承したclassとする 前の座標からの配置をbase(FRAME) で表現する. jointによる運動をbaseからの関節パラメタによる座標変換で表現する linkの形状はvframeの中で表現する class Link(PartsObject) : def init (self) : PartsObject. init (self) self.base=frame() self.type = None self.sc =1.0 self.jparam=0.0 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 8
- link joint の例 次のリンク self.vbody self self.jparam self.rel_trans self.base 前のリンク 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 9
- joint の設定 座標変換を指定して親リンクに affix する link.type に 0-5 の整数で指定することで x, y, z の並進またはその周りの回転関節を表す def place_joint(self, prnt, trans, type) : self.type=type tmp = VECTOR() if 0 <= self.type < 3 : tmp[self.type]=1.0 self.j_axis=tmp self.j_kind=0 # 0:translation elif 3 <= self.type < 6 : tmp[self.type-3]=1.0 self.j_axis=tmp self.j_kind=1 # 1:rotational else : print "error: joint type" self.affix(prnt,trans,'rigid') self.base=trans 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 10
- joint の操作 self.typenの0から5の数値を利用して 並進 回転のFRAMEを作り set_transする 単位は 基本は並進はメートル ( のつもり ) 回転はラジアン self.scでスケール ( 単位 ) を指定することができる def set_jparam(self, val) : self.jparam=self.sc*val param=[0,0,0,0,0,0] param[self.type]=self.jparam self.set_trans(self.base*frame(xyzabc=param)) 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 11
- 多関節アームの表現 (LinkedArm) linkの連接でアームを表現する base wristおよびhandを持つ 形状や軸配置は煩雑なので雛形を生成後に指定する class LinkedArm : def init (self,dof) : self.dof = dof self.links=[] self.base = PartsObject() for i in range(dof) : self.links.append(link()) self.wrist = PartsObject() self.tools={} self.hand=none... # base # wrist 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 12
- LinkedArm のイメージ self.wrist self.links[ ] self.hand self.base 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 13
- 多関節アームの表現 (2) 関節パラメタをまとめて扱う def get_joints(self): tmp=[] for i in range(self.dof) : tmp.append(self.links[i].jparam) return tmp def set_joints(self,params) : for i in range(self.dof): self.links[i].set_jparam(params[i]) 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 14
- 3 関節アームの例 (1) まずは形状なしのスケルトン skelton3dof.py l 3 l 2 l 1 l b hand wrist link 3 link 2 link 1 base joint 3 joint 2 joint 1 from larm_w_hand_arm_sol import * # arm = LinkedArm(3) # arm.bh = 0.1 # arm.l1h = 0.05 arm.links[0].place_joint(arm.base, FRAME(xyzabc=[0,0,arm.bh,0,0,0]),5) # arm.l2h = 0.4 arm.links[1].place_joint(arm.links[0],frame(xyzabc=[0,0, arm.l1h,0,0,0]),4) # arm.l3h = 0.4 arm.links[2].place_joint(arm.links[1],frame(xyzabc=[0,0, arm.l2h,0,0,0]),4) # arm.wrist.affix(arm.links[2],frame(xyzabc=[0,0, arm.l3h,0,0,0])) 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 15
- 3 関節アームの例 (2) hand wrist l 3 link 3 joint 3 l 2 link 2 l 1 l b link 1 base joint 2 joint 1 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 16
- 3 関節アームの例 (3) >>> arm.set_joints([pi/6,pi/4,pi/2]) >>> arm.wrist.where() f:(m:[[-0.61237243569579447, -0.49999999999999994, 0.61237243569579458], [-0.35355339059327368, 0.86602540378443871, 0.35355339059327373], [-0.70710678118654757, 0.0, -0.70710678118654746]],v:[0.48989794855663565, 0.28284271247461901, 0.15000000000000008]) x Base 1 = 6 y 2 = 4 3 = 2 l3h l2h z l1h bh x L1 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 17
- 形状を付ける arm3dof.py import new from larm_w_hand_arm_sol import * from hand import * def create_arm() : hand = create_hand() arm = LinkedArm(3,hand=hand) クラスの中身の操作が出来るモジュール ハンドのモジュールハンドの生成 外で定義した関数をクラスのメソッドにする arm.make_shape=new.instancemethod(make_shape,arm,arm. class ) arm.ready_angle=[0,pi/4,pi/2] arm.park_angle=[0,0,0] arm.mark=new.instancemethod(mark_arm,arm,arm. class ) arm.solve=new.instancemethod(arm_sol,arm,arm. class ) arm.solve2=new.instancemethod(arm_sol2,arm,arm. class ) arm.move=new.instancemethod(arm_move,arm,arm. class ) arm.make_shape() return arm 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 18
- 形状を付ける def make_shape(self) : self.bh = 0.1 self.br = 0.1 self.base.vbody=visual.cylinder(axis=(0.0, 0.0, self.bh ), radius=self.br) self.base.vbody.frame = self.base.vframe # self.l1r = 0.1 self.l1h = 0.05 self.links[0].set_vbody(visual.cylinder(axis=(0.0, 0.0, self.l1h ), radius=self.l1r)) self.links[0].place_joint(self.base, FRAME(xyzabc=[0,0,self.bh,0,0,0]),5) # self.l2r = 0.04 self.l2h = 0.4 self.links[1].set_vbody(visual.cylinder(axis=(0.0, 0.0, self.l2h ), radius=self.l2r)) self.links[1].place_joint(self.links[0], FRAME(xyzabc=[0,0, self.l1h,0,0,0]),4) # self.l3r = 0.02 self.l3h = 0.4 self.links[2].set_vbody(visual.cylinder(axis=(0.0, 0.0, self.l3h ), radius=self.l3r)) self.links[2].place_joint(self.links[1], FRAME(xyzabc=[0,0, self.l2h,0,0,0]),4) # self.wrist.set_vbody(visual.box(length=0.04,height=0.04,width=0.02)) self.wrist.affix(self.links[2],frame(xyzabc=[0,0, self.l3h,0,0,0])) 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 19
- ハンド 物をつかむときに掴みやすいように座標系を設定する. z x y x z box.grip y hand box 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 20
- ハンド アームの手首部に装着したときに, その座標系がどのような状態になるかを考えて座標変換行列を設定する. hand y z x z hand.set_trans(frame(xyzabc=[0,0,0.09,pi,0,0])) wrist x y 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 21
- hand.py (1) from time import * from object_model_v import * import new def create_hand() : hand = Hand() hand.body=visual.box(size=(0.03,0.06,0.04),color=visual.color.green) hand.finger1=visual.box(size=(0.03,0.004,0.05),color=visual.color.green) hand.finger2=visual.box(size=(0.03,0.004,0.05),color=visual.color.green) hand.body.pos=(0,0,0.07) hand.finger1.pos=(0,0.002,0.025) hand.finger2.pos=(0,-0.002,0.025) hand.body.frame=hand.vframe hand.finger1.frame=hand.vframe hand.finger2.frame=hand.vframe 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 22
- hand.py (2) def hand_open(self,width) : self.finger1.pos=(0,width/2.0+0.002,0.025) self.finger2.pos=(0,-width/2.0-0.002,0.025) sleep(0.5) hand.open=new.instancemethod(hand_open,hand,hand. class ) def hand_close(self,width=0.0) : self.finger1.pos=(0,width/2.0+0.002,0.025) self.finger2.pos=(0,-width/2.0-0.002,0.025) sleep(0.5) hand.close=new.instancemethod(hand_close,hand,hand. class ) hand.set_trans(frame(xyzabc=[0,0,0.09,pi,0,0])) return hand 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 23
- ハンドだけ作ってみる >>> hand=create_hand() >>> 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 24
- 指の開閉 >>> hand.open(0.05) >>> >>> hand.close() >>> 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 25
- 3 関節アーム形状, ハンド付き >>> arm=create_arm() >>> hand wrist l 3 link 3 joint 3 l 2 link 2 l 1 l b link 1 base joint 2 joint 1 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 26
- 3 関節アーム形状付き (3) hand wrist >>> arm.set_joints([pi/6,pi/4,pi/2]) >>> arm.wrist.where().xyzabc() [0.48989794855663565, 0.28284271247461901, 0.15000000000000008, -2.677945044588987, 0.65905803582640898, 2.4568734505875103] >>> arm.hand.where().xyzabc() [0.54501146776925713, 0.31466251762801367, 0.086360389693210804, 0.46364760900080626, -0.65905803582640887, -2.4568734505875103] l 3 link 3 joint 3 l 2 link 2 l 1 l b link 1 base joint 2 joint 1 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 27
- 座標軸を消して アニメをする >>> AxesXYZ.visible_all(0) >>> for i in range(100) : th1=pi/6+(pi/100)*i th2=pi/4-(pi/200)*i th3=pi/2-(pi/100)*i arm.set_joints([th1,th2,th3]) time.sleep(0.1) >>> 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 28
- 環境モデル作成 (env_arm3dof.py) from object_model_v import * from arm3dof import * def create_env() : global room, table, place_a, place_b, box, up200, arm, hand room = CoordinateObject() table_body=visual.box(color=visual.color.blue,width=0.02,length=0.5,height=0.7) table_body.pos=(0,0,-0.01) table = PartsObject(vbody=table_body) table.affix(room,frame(xyzabc=[0,0,0.5,0,0,pi/6])) place_a = CoordinateObject() place_a.affix(table,frame(xyzabc=[0.1,0,0,0,0,0])) place_b = CoordinateObject() place_b.affix(table,frame(xyzabc=[-0.1,0,0,0,0,-pi/6])) box_body=visual.box(width=0.1, length=0.05, height=0.03) box_body.pos=(0,0,0.05) box = PartsObject(vbody=box_body) box.affix(table,place_a.where(table)) up200=frame(vec=vector(0,0,0.2)) arm=create_arm() arm.base.affix(table,frame(vec=vector(-0.15,0.25,0))) hand=arm.hand 形状の原点と座標系の原点の差を調整する 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 29
- 環境を取り込む env_arm3dof.py >>> create_env() >>> 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 30
- 例題 13-1:wrist を box の位置へ動かす >>>??? 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 31
- 例題 13-1:wrist を box の位置へ動かす >>>??? >>> arm.hand.where(box).xyzabc() [0.0019463287112361227, 0.00075918764870426536, 0.0009779721930747165, -0.17883897664077739, -0.17791679759437939, 2.3455990146425036] >>> arm.get_joints() [-0.78000000000000003, 0.62, 2.27] >>> 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 32
- 7 自由度アーム (pa10)+ ハンド 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 33
- 例題 13-2:6 関節アームを作る hand wrist base l h =0.09 l 6 =0.05 l 5 =0.1 l 4 =.01 l 3 =0.3 l 2 =0.4 l 1 =0.05 l b =0.1 (1) 6 関節アームを作る arm6dof.py 関節配置 リンク長は左図のとおり形状は適切に 関節パラメタが全部 0のとき ベース座標系と手首座標系の姿勢が一致している ハンドと手首の姿勢はx 軸回りに 180 度まわす (x 軸は一致 y,z 軸は反対方向になる ) 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 34
- arm6dof_tmpl.py #!/usr/bin/env python # -*- Python -*- # import sys from larm_w_hand_arm_sol_new import * from hand import * # # instanciate 6dof arm # def create_arm() : hand = create_hand() arm = LinkedArm(6,hand=hand) arm.ready_angle=[0,pi/6,pi/2,0,pi/3,0] arm.park_angle=[0,0,0,0,0,0] make_shape(arm) return arm 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 35
- arm6dof_tmpl.py def make_shape(self) : # base self.bh = 0.1 # self.br = 0.1 # self.base.vbody=visual.cylinder(axis=(0.0, 0.0, self.bh ), radius=self.br) # self.base.vbody.frame = self.base.vframe # # link 1 self.l1h = 0.05 # self.l1r = 0.1 # self.links[0].set_vbody(visual.cylinder(axis=(0.0, 0.0, self.l1h ), # radius=self.l1r)) # self.links[0].place_joint(self.base, FRAME(xyzabc=[0,0,self.bh,0,0,0]),5) # # link2 self.l2h = 0.4 # link3 self.l3 = 0.3 # link4 self.l4h = 0.1 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 36
- arm6dof_tmpl.py # link6 self.l6h = 0.05 # wrist # self.wrist.set_vbody(visual.box(length=0.04,height=0.04,width=0.02,pos=(0,0,-0.01))) # self.wrist.affix(self.links[5],frame(xyzabc=[0,0, self.l6h,0,0,0])) # end of make_shape # 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 37
- env_arm6dof_tmpl.py from time import * from object_model_v import * from arm6dof_tmpl import *... def go_to_box(): a=[pi/6,pi/4,pi/2,0,0,pi/3] arm.set_joints(a) return arm.hand.where(box).xyzabc() 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 38
- 次回予告 手先を望みの場所 姿勢にするには? => 逆運動学の問題 2015 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 39